• 阅读数:310发布于2021-01-13 17:32:10

    只看该作者
    5张彩色图+5张深度图+PCL= 点云拼接 复制本文链接

    主要是用已知的图片(5张彩色+5张深度图)+ pose.txt(相机位姿-> 前三位是xyz轴方向上的平移量,后四位是旋转四元数实部+虚部; 用以上数据得到点云
    思路: 先用位姿数据计算得出旋转矩阵T
    而后根据图片的像素坐标以及相机内参计算得出实物的相机坐标;最后将相机坐标转换为世界坐标
    操作注意事项:

    1. 先在build文件夹路径下编译源码 得到可执行文件;
    2. 将可执行文件复制到 pose.txt路径下(该路径下还有存放彩色图和深度图的文件夹) 而后执行./joinMap 得到.pcd 地图;
    3. 执行pcl_viewer map.pcd 用可视化程序显示拼接的地图。
      #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>
      
      int main( int argc, char** argv )
      {
          vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图 每张图用矩阵存储 多张图以向量形式表示
          vector<Eigen::Isometry3d> poses;   //欧式变换矩阵  4 *4    // 相机位姿
          //读操作(输入)的文件类
          ifstream fin("./pose.txt");//每行前三个数是平移(xyz) 后四个是四元数 指的是相机姿态
          if (!fin)
          {
              cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
              return 1;
          }
      
          for ( int i=0; i<5; i++ )
          {
              //boost::format 类似C++中的printf
              boost::format fmt( "./%s/%d.%s" ); //图像文件格式
              colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));//str() 函数将对象转化为适于人阅读的形式。
              depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
      
              double data[7] = {0};
              for ( auto& d:data )
                  fin>>d;
              //最后一位是四元数实部
              Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
              Eigen::Isometry3d T(q);
              T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
              poses.push_back( T );//T是 相机位姿(装的是5张图的位姿)
          }
      
          // 计算点云并拼接
          // 相机内参 
          double cx = 325.5;
          double cy = 253.5; //平移
          double fx = 518.0;
          double fy = 519.0;//缩放×焦距
          double depthScale = 1000.0;
          cout<<"正在将图像转换为点云..."<<endl;
      
          // 新建一个点云//PointCoud::Ptr是一个智能指针类 通过构造函数初始化指针指向的申请的空间
             /*Ptr是一个智能指针,返回一个PointCloud<PointT> 其中PointT是pcl::PointXYZRGB类型。它重载了->  返回了指向PointCloud<PointT>的指针
           *Ptr是下面类型 boost::shared_ptr<PointCloud<PointT> > */
              /*pointCloud 是一个智能指针类型的对象 具体可以参考http://blog.csdn.net/worldwindjp/article/details/18843087*/
          // 定义点云使用的格式:这里用的是XYZRGB
          typedef pcl::PointXYZRGB PointT; 
          typedef pcl::PointCloud<PointT> PointCloud;
      
          // 新建一个点云
          PointCloud::Ptr pointCloud( new PointCloud ); 
          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];
              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]; // 深度值(第v行u列)
                      if ( d==0 ) continue; // 为0表示没有测量到
                      Eigen::Vector3d point; //像素坐标
                      point[2] = double(d)/depthScale; //Z 深度
                      point[0] = (u-cx)*point[2]/fx;//x坐标
                      point[1] = (v-cy)*point[2]/fy; 
                      Eigen::Vector3d pointWorld = T* point ;//将相机坐标系下的坐标变换到世界坐标系
                      PointT p ;
                      p.x = pointWorld[0];
                      p.y = pointWorld[1];
                      p.z = pointWorld[2];
                      /*  color.step 虽然是一个类,但是它内部有一个转换操作符 operator size_t() const;
      131                  * 此时的color.size编译器就会把它当做size_t类型的变量,这个值的大小是1920 这个是随着图像的读入MAT类中会有自动转换然后存储的buf[]中 */
                      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 );
                  }
          }
          pointCloud->is_dense = false;
          cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
          pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
          return 0;
      }
      


    回复 (1)

    举报
举报

请选择举报理由

  • 垃圾广告
  • 违规内容
  • 恶意灌水
  • 重复发帖
提示

奥比中光 · 3D视觉开发者社区...

站长统计