ORBSLAM 使用深度相机稠密重建后地图上下颠倒的解决办法

更改pointcloudmapping.cc文件中的变换参数：

pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud(KeyFrame* kf, cv::Mat& color, cv::Mat& depth)
{
    PointCloud::Ptr tmp( new PointCloud() );
    // point cloud is null ptr
    for ( int m=0; m<depth.rows; m+=3 )
    {
        for ( int n=0; n<depth.cols; n+=3 )
        {
            float d = depth.ptr<float>(m)[n];
            if (d < 0.01 || d>10)
                continue;
            PointT p;
            p.z = d;
            p.x = ( n - kf->cx) * p.z / kf->fx;
            p.y = ( m - kf->cy) * p.z / kf->fy;
            
            p.b = color.ptr<uchar>(m)[n*3];
            p.g = color.ptr<uchar>(m)[n*3+1];
            p.r = color.ptr<uchar>(m)[n*3+2];
                
            tmp->points.push_back(p);
        }
    }
  
    // rotation the pointcloud and stiching 
    Eigen::Isometry3d T = ORB_SLAM2::Converter::toSE3Quat( kf->GetPose() );
    PointCloud::Ptr cloud(new PointCloud);
    pcl::transformPointCloud( *tmp, *cloud, T.inverse().matrix());
    cloud->is_dense = false;
    voxel.setInputCloud( cloud );
    voxel.filter( *tmp );
    cloud->swap( *tmp );
    
       // save pointcloud to disk 
    //mvPosePointClouds.push_back(kf->GetPose());
    //mvPointClouds.push_back(*tmp);
    //mpointcloudID++;
    //stringstream ss;
    //ss << "/home/crp/dataset/rgbd_dataset_freiburg1_room/pointcloud/"<< mpointcloudID<<".pcd";
    //pcl::io::savePCDFile(ss.str(),*tmp);

     
    cout<<"generate point cloud from  kf-ID:"<<kf->mnId<<", size="<<cloud->points.size()<<endl;
     
    return cloud;
}
/*
 * @function 更加关键帧生成点云、并对点云进行滤波处理
 * 
 * 备注：点云生成函数在　台式机上调用时间在0.1ｓ 左右
 */
pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud( cv::Mat& color, cv::Mat& depth)
{
     chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    PointCloud::Ptr tmp( new PointCloud() );// point cloud is null ptr
    
    for ( int m=0; m<depth.rows; m+=3 )
    {
        for ( int n=0; n<depth.cols; n+=3 )
        {
            float d = depth.ptr<float>(m)[n];
            if (d < 0.01 || d>10)
                continue;
            PointT p;
            p.z = d;
            p.x = ( n - cx) * p.z / fx;
            p.y = ( m -cy) * p.z / fy;
            
            p.b = color.ptr<uchar>(m)[n*3];
            p.g = color.ptr<uchar>(m)[n*3+1];
            p.r = color.ptr<uchar>(m)[n*3+2];
                
            tmp->points.push_back(p);
        }
    }
// rotation the pointcloud and stiching 
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity() ;
    PointCloud::Ptr cloud1(new PointCloud);
    PointCloud::Ptr cloud2(new PointCloud);
    pcl::transformPointCloud( *tmp, *cloud1, T.inverse().matrix());
     pcl::transformPointCloud( *tmp, *cloud2, T.inverse().matrix()); //专用于点云匹配
     
    cloud1->is_dense = false;
    voxel.setInputCloud( cloud1 );
    voxel.filter( *tmp );
    cloud1->swap( *tmp );
    
  
     cloud2->is_dense = false;
    voxelForMatch.setInputCloud( cloud2 );
    voxelForMatch.filter( *tmp );
    cloud2->swap( *tmp );
   // addVertexToOptimizer(mpointcloudID,mpose);
   
    mvPointClouds.push_back(cloud1) ;    //点云数据 
     mvPointCloudsForMatch.push_back(cloud2) ;    
    mpointcloudID++;
     cout<<"generate point cloud from  kf-ID:"<<mpointcloudID<<", size="<<cloud1->points.size();
     
    //stringstream ss;
    //ss << "/home/crp/dataset/rgbd_dataset_freiburg1_room/pointcloud/"<< mpointcloudID<<".pcd";
    //pcl::io::savePCDFile(ss.str(),*tmp);
  
     chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
     chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
     cout<<"   cost time: "<<time_used.count()*1000<<" ms ."<<endl;
     
    return cloud1;
}

其中p.x,p.y,p.z的值需要取反等操作。

            p.z = -d;
            p.x = ( n - kf->cx) * p.z / kf->fx;
            p.y = ( m - kf->cy) * p.z / kf->fy;