JoinMap

#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
//#include <pcl/visualization/pcl_visualizer.h>

//#define EIGEN_DONT_VECTORIZE
//#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT

int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿

    ifstream fin("./pose.txt");//读取相机位姿 如果没有相机位姿呢？相机位姿在哪里？
    if (!fin)
    {
        cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
        //return 1;
    }

    for ( int i=; i<; i++ )
    {
        boost::format fmt( "./%s/%d.%s" ); //图像文件格式
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+)%"jpg").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+)%"jpg").str(), - )); // 使用-1读取原始图像

        double data[] = {};
        for(int j=;j<;j++)
        {
          double d=;
          fin>>d;
          data[j]=d;
        }
        //for ( auto& d:data )
            //fin>>d;
        Eigen::Quaterniond q( data[], data[], data[], data[] ); //旋转4变量
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[], data[], data[] )); //平移3变量
        poses.push_back( T );
    }

    // 计算点云并拼接
    // 相机内参
    double cx = 315.9;// 319.5
    double cy = 230.3;// 239.5
    double fx = 371.5;// 490.3
    double fy = 374.2;// 490.3
    double depthScale = 1000.0;

    cout<<"正在将图像转换为点云..."<<endl;

    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    // 新建一个点云
    PointCloud::Ptr pointCloud( new PointCloud );
    for ( int i=; i<; i++ )
    {
        cout<<"转换图像中: "<<i+<<endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i]; //姿态
        for ( int v=; v<color.rows; v++ )
            for ( int u=; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                if ( d== ) continue; // 为0表示没有测量到
                Eigen::Vector3d point;
                point[] = double(d)/depthScale;  //深度值除以1000
                point[] = (u-cx)*point[]/fx; //X
                point[] = (v-cy)*point[]/fy; //Y
                Eigen::Vector3d pointWorld = *point; //转世界坐标

                PointT p ;
                p.x = pointWorld[]; //世界坐标x
                p.y = pointWorld[]; //世界坐标y
                p.z = pointWorld[]; //世界坐标z
                p.b = color.data[ v*color.step+u*color.channels() ]; //颜色b
                p.g = color.data[ v*color.step+u*color.channels()+ ]; //颜色g
                p.r = color.data[ v*color.step+u*color.channels()+ ]; //颜色r
                pointCloud->points.push_back( p );
            }
     }

    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    system("pause");
    return ;
}

VS生成的C++项目常用目录设置:https://blog.csdn.net/CreateUserName_Keep/article/details/78884195

VS2010项目属性的默认包含路径设置方法：

可是用cloud_viewer.exe map.pcd在命令行却说“应用程序无法正常启动0xc000007b”，这一定是因为pcl的外部链接库没有添加到path路径或者是缺少dll文件导致的。但是在pcl目录下却可以

这个很明显不大对

https://blog.csdn.net/u012423865/article/details/78036543

https://blog.csdn.net/blademan1234/article/details/79796901

问题：真实距离是多少？为什么绘制的效果很差。。。噪声太多？

操作：