SLAM第五讲点云拼接思路笔记

1.用vector<cv::Mat>和vector<Eigen::Isometry3d>定义图片变量和相机位姿。

    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;

容器的基本使用：http://blog.youkuaiyun.com/ws_20100/article/details/50829327

2.读取

（1）先设地址，位姿地址就一个。用

 ifstream fin("../pose.txt")

直接读取。

图片地址有多个，需要在for循环里，先boost::format fmt("../%s/%d.%s")，把图像文件格式统一一下。

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

其实就是找到图像存储的路径，然后放入先前创建好的vector中。cv::imread读的是参数，所以fmt还要再.str()将其变为字符串。

（2）位姿数据里面定义的有7个量，前面3个是位移，后面四个是四元数，最后一个是实部。

定义一个7个变量的数组并初始化为0，然后定义一引用，一个for循环，让for循环遍历data中的每一元素d,并给每个元素赋位姿里的值。

 double data[7] = {0};
 for ( auto& d:data )//auto自动类型转换
 fin>>d;//文件流类型的变量fin将pose.txt中的数据给了d数组,//将上面的每个图片的外参都给了d
 Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //四元数 data[6]是实数 但是coeffs输出的是先虚数后实数
 Eigen::Isometry3d T(q);                                     //变换矩阵初始化旋转部分，
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//变换矩阵初始化平移向量部分
poses.push_back( T );   //存储变换矩阵到位姿数组

在这里简介一下auto：编程时常常需要把表达式的值赋给变量，这就要求在声明变量的时候清楚知道表达式的类型。然而要做到这一点并非那么容易，有时候甚至根本做不到。为了解决这个问题，C++11标准引入了 auto类型说明符，用它就能让编译器替我们去分析表达式所属的类型。

参考链接：c++11 for http://blog.youkuaiyun.com/hackmind/article/details/24271949

用这7个值构成一个位姿T.

放在i循环下，说明i=1的时候对应一个位姿，每个I对应的位姿并不相同。

3.定义内参和尺度

    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    cout<<"正在将图像转换为点云..."<<endl;

4.定义ＲＧＢ值PointT和点云pointcloud

    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;

5.新建一个点云

// 新建一个点云,PointCoud::Ptr是一个智能指针类 通过构造函数初始化指针指向的申请的空间

//pointCloud 是一个智能指针类型的对象 
PointCloud::Ptr pointCloud( new PointCloud );

//    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZRGB> );

6.由图片得到像素坐标。接下来通过像素坐标计算出像素在相机坐标系下的坐标。公式简单。
z=d/尺度因子。
d是每个像素对应的深度值，可以通过depth.ptr<unsigned short>读取出来。

x=(u-cx)*z/fx;y=(u-cy)*z/fy.得到。
然后通过相机位姿算出在世界坐标系下的坐标。

定义一个pcl点p.这个p有六个值，x,y,z，b,g,r。x,y,z就是世界坐标系下的坐标，b,g,r就是颜色值。这6个值构成了点p.点云的points.push_back()把一个一个点P放进去，就构成了点云。

    for ( int i=0; i<5; i++ )
    {
        cout<<"转换图像中: "<<i+1<<endl; 
        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        //

对图像像素进行坐标转换，将图像的坐标通过内参矩阵K转换为相机坐标系下的坐标，之后通过外参矩阵T 转化为世界坐标系下的坐标        
      for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
              unsigned int d = depth.ptr<unsigned short> ( v )[u]; //深度值，因为深度图是单通道的，每个像素的值是unsigned short，所以参数是unsigned short

            if ( d==0 ) continue; // 为0表示没有测量到,为0表示没有测量到 然后继续进行for循环那么跳过这个像素继续执行 在后面形成点云时需要设置is_dense为假

         Eigen::Vector3d point;
         point[2] = double(d)/depthScale;
         point[0] = (u-cx)*point[2]/fx; 
         point[1] = (v-cy)*point[2]/fy;
         Eigen::Vector3d pointWorld = T*point;

    //将相机坐标系转换为世界坐标系
       PointT p ;
       p.x = pointWorld[0];  //

 //将世界坐标系下的坐标用pcl专门的点云格式存储起来        
     p.y = pointWorld[1];        
     p.z = pointWorld[2];        
     p.b = color.data[ v*color.step+u*color.channels() ];        
     p.g = color.data[ v*color.step+u*color.channels()+1 ];        
     p.r = color.data[ v*color.step+u*color.channels()+2 ];   
   //与存储结构相关      
     pointCloud->points.push_back( p );
    //points是pointCloud中的一个结构体
 }

7.点云拼接

    

   pointCloud->is_dense = false;//

这里有可能深度图中某些像素没有深度信息，那么就是包含无效的像素，所以先置为假
 cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
 pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
 return 0;