稠密ORBSLAM修改
修改CmakeLists，新增pcl库的依赖
ind_package(OpenCV 4.3.0 REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Pangolin REQUIRED)
find_package( G2O REQUIRED )
find_package(Boost REQUIRED)
find_package( PCL REQUIRED )
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${BOOST_INCLUDE_DIRS}
)
add_definitions( ${PCL_DEFINITIONS} )
link_directories( ${PCL_LIBRARY_DIRS} ${Boost_LIBRARIES} )
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension g2o_types_sim3 g2o_types_sba
${PCL_LIBRARIES}
${Boost_LIBRARYES}
)
增加PointCloudMapping类。
初始化 PointCloudMapping
插入关键帧 insertKeyFrame
产生相机坐标系点云 generatePointCloud
BA更新点云 updatecloud
保存 save
显示 viewer（将相机坐标系下的点云转换到世界坐标系下并显示出来）
关闭 shutdown
pointcloudmapping.h
#ifndef POINTCLOUDMAPPING_H
#define POINTCLOUDMAPPING_H
#include "System.h"
#include
#include
#include
#include
#include
using namespace ORB_SLAM2;
class PointCloudMapping
{
public:
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud PointCloud;
PointCloudMapping( double resolution_ );
// 插入一个keyframe，会更新一次地图
void insertKeyFrame( KeyFrame* kf, cv::Mat& color, cv::Mat& depth );
void shutdown();
void viewer();
protected:
PointCloud::Ptr generatePointCloud(KeyFrame* kf, cv::Mat& color, cv::Mat& depth);
PointCloud::Ptr globalMap;
shared_ptr viewerThread;
bool shutDownFlag =false;
mutex shutDownMutex;
condition_variable keyFrameUpdated;
mutex keyFrameUpdateMutex;
// data to generate point clouds
vector keyframes;
vector colorImgs;
vector depthImgs;
mutex keyframeMutex;
uint16_t lastKeyframeSize =0;
double resolution = 0.04;
pcl::VoxelGrid voxel;
};
#endif // POINTCLOUDMAPPING_H
pointcloudmapping.cc
#include "pointcloudmapping.h"
#include
#include
#include
#include "Converter.h"
#include
PointCloudMapping::PointCloudMapping(double resolution_)
{
this->resolution = resolution_;
voxel.setLeafSize( resolution, resolution, resolution);
//globalMap = boost::shared_ptr< PointCloud >(new PointCloud());
globalMap = boost::make_shared< PointCloud >();
viewerThread = make_shared( bind(&PointCloudMapping::viewer, this ) );
}
void PointCloudMapping::shutdown()
{
{
unique_lock lck(shutDownMutex);
shutDownFlag = true;
keyFrameUpdated.notify_one();
}
viewerThread->join();
}
void PointCloudMapping::insertKeyFrame(KeyFrame* kf, cv::Mat& color, cv::Mat& depth)
{
cout<<"receive a keyframe, id = "<mnId< lck(keyframeMutex);
keyframes.push_back( kf );
colorImgs.push_back( color.clone() );
depthImgs.push_back( depth.clone() );
keyFrameUpdated.notify_one();
}
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(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(m)[n*3];
p.g = color.ptr(m)[n*3+1];
p.r = color.ptr(m)[n*3+2];
tmp->points.push_back(p);
}
}
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;
cout<<"generate point cloud for kf "<mnId<<", size="<points.size()< lck_shutdown( shutDownMutex );
if (shutDownFlag)
{
break;
}
}
{
unique_lock lck_keyframeUpdated( keyFrameUpdateMutex );
keyFrameUpdated.wait( lck_keyframeUpdated );
}
// keyframe is updated
size_t N=0;
{
unique_lock lck( keyframeMutex );
N = keyframes.size();
}
for ( size_t i=lastKeyframeSize; iswap( *tmp );
viewer.showCloud( globalMap );
cout<<"show global map, size="<points.size()< 系统入口
system.h和system.cc：
增加头文件
#include "pointcloudmapping.h"
在ORB__SLAM2的命名空间里加入一行声明
class PointCloudMapping;
增加private成员
shared_ptr mpPointCloudMapping; 1
创建PointCloudMapping对象，用共享指针make_shared保存，并在初始化Tracking线程的时候传入
mpPointCloudMapping = make_shared( 0.01 );
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor, mpPointCloudMapping); Shutdown函数加入mpPointCloudMapping->requestFinish();，接下去的while循环中对!mpPointCloudMapping->isFinished()进行判断，如果稠密点云图的创建还未结束，则暂时还不能结束系统。
void System::Shutdown()
{
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
if(mpViewer)
{
mpViewer->RequestFinish();
while(!mpViewer->isFinished())
usleep(5000);
}
mpPointCloudMapping->requestFinish();
// Wait until all thread have effectively stopped
while(!mpPointCloudMapping->isFinished() || !mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA())
{
usleep(5000);
}
if(mpViewer)
pangolin::BindToContext("ORB-SLAM2: Map Viewer");
}增加获取稠密点云地图的方法，调用的是tracking类的getPointCloudMap()方法，并由outputMap保存（注意传入的是引用）。
void System::getPointCloudMap(pcl::PointCloud ::Ptr &outputMap)
{
mpTracker->getPointCloudMap(outputMap);
} 跟踪线程
Tracking.h和Tracking.cc：
增加pcl相关头文件
#include "PointcloudMapping.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
添加protected成员变量
cv::Mat mImRGB;
cv::Mat mImDepth;
shared_ptr mpPointCloudMapping; 构造函数中增加参数shared_ptr
在Tracking::GrabImageRGBD()中保存RGB和Depth图像
mImRGB=imRGB;
mImDepth=imDepth;
在Tracking::CreateNewKeyFrame() 中将关键帧插入到点云地图
mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth );
增加获取稠密点云地图的方法，调用的是PointCloudMapping类的getGlobalCloudMap()方法。
void Tracking::getPointCloudMap(pcl::PointCloud::Ptr &outputMap)
{
mpPointCloudMapping->getGlobalCloudMap(outputMap);
}