视觉SLAM14讲使用第八章深度图片进行点云图绘制

第八章中有9章深度图，能否进行点云图的绘制呢，像第五章一样，我进行了尝试，明天会下载整个数据集更新的。

目前只用九组深度图片。

cmakelist:

cmake_minimum_required( VERSION 2.8 )

project( test_read )
set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )

# opencv 
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

# eigen 
include_directories( "/usr/include/eigen3/" )

# pcl 
find_package( PCL 1.9 REQUIRED COMPONENT common io )
list (REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
add_executable( test_read main.cpp )

target_link_libraries( test_read ${OpenCV_LIBS} ${PCL_LIBRARIES} )

cpp:

#include <iostream>
#include <fstream>
#include <string>
#include <list>
#include <vector>
#include <chrono>
#include <sstream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>


#include <iostream>
#include <fstream>
#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> 

double string_to_double(string data_string)
{
  stringstream ss;
  double data_double;
  ss<<data_string;
  ss>>data_double;
  return(data_double);
}

int main( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    string path_to_dataset = argv[1];
    string associate_file1 = path_to_dataset + "/associate.txt";
    string associate_file2 = path_to_dataset + "/groundtruth.txt";
    ifstream fin1( associate_file1 );
    if ( !fin1 ) 
    {
        cerr<<"I cann't find associate.txt!"<<endl;
        return 1;
    }
    
    ifstream fin2( associate_file2 );
    if ( !fin2 ) 
    {
        cerr<<"I cann't find groundtruth.txt!"<<endl;
        return 1;
    }
    
    string rgb_file, depth_file, time_rgb, time_depth;    
    cv::Mat color, depth, last_color;
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿
    double time_error;int flag=0;
    for ( int index=0; index<9; index++ )
    {
	flag=0;  
	fin1>>time_rgb>>rgb_file>>time_depth>>depth_file;
        color = cv::imread( path_to_dataset+"/"+rgb_file );
	colorImgs.push_back( color );
        depth = cv::imread( path_to_dataset+"/"+depth_file, -1 );
        depthImgs.push_back( depth ); // 使用-1读取原始图像
	
	for(;flag==0;)
	{
	  double data[8] = {0};
	  for ( auto& d:data )
	    fin2>>d;
	  if(abs(2*data[0]-string_to_double(time_rgb)-string_to_double(time_depth))<0.01)
	  {
	    cout<<data[0]<<endl;
	    Eigen::Quaterniond q( data[7], data[4], data[5], data[6] );
	    Eigen::Isometry3d T(q);
	    T.pretranslate( Eigen::Vector3d( data[1], data[2], data[3] ));
	    poses.push_back( T );
	    flag=1;
	  }
	}
    }
    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    
    cout<<"正在将图像转换为点云..."<<endl;
    
    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 新建一个点云
    PointCloud::Ptr pointCloud( new PointCloud ); 
    for ( int i=0; i<9; 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]; // 深度值
                if ( d==0 ) continue; // 为0表示没有测量到
                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];
                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 );
            }
    }
    
    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}

效果：