代码从哪里抄过来?
一、还有什么要补充的吗?
要完全补全还需要结合另外一篇文章
彩色地图和点云保存代码补全参考这里
二、操作步骤
1.引入库
CMakeLists.txt
# adding for point cloud viewer and mapper
find_package( PCL 1.7 REQUIRED )
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
add_definitions( ${PCL_DEFINITIONS} )
link_directories( ${PCL_LIBRARY_DIRS} )
add_library(${PROJECT_NAME} SHARED
src/System.cc
src/Tracking.cc
src/LocalMapping.cc
src/LoopClosing.cc
src/ORBextractor.cc
src/ORBmatcher.cc
src/FrameDrawer.cc
src/Converter.cc
src/MapPoint.cc
src/KeyFrame.cc
src/Map.cc
src/MapDrawer.cc
src/Optimizer.cc
src/PnPsolver.cc
src/Frame.cc
src/KeyFrameDatabase.cc
src/Sim3Solver.cc
src/Initializer.cc
src/Viewer.cc
src/pointcloudmapping.cc //增加此处
)
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
${PCL_LIBRARIES}
)
2.配置文件增加一条
Examples/RGB-D/TUM1.yaml
PointCloudMapping.Resolution: 0.001//增加此处
最末尾
3.源代码增删
建立新的文件
可以直接从ORBSLAM2_with_pointcloud_map复制
include/pointcloudmapping.h
#ifndef POINTCLOUDMAPPING_H
#define POINTCLOUDMAPPING_H
#include "System.h"
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <condition_variable>
#include <pcl/io/pcd_io.h>
using namespace ORB_SLAM2;
class PointCloudMapping
{
public:
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;
PointCloudMapping( double resolution_ );
// 插入一个keyframe,会更新一次地图
void insertKeyFrame( KeyFrame* kf, cv::Mat& color, cv::Mat& depth );
void shutdown();
void viewer();
void save();
protected:
PointCloud::Ptr generatePointCloud(KeyFrame* kf, cv::Mat& color, cv::Mat& depth);
PointCloud::Ptr globalMap;
shared_ptr<thread> viewerThread;
bool shutDownFlag =false;
mutex shutDownMutex;
condition_variable keyFrameUpdated;
mutex keyFrameUpdateMutex;
// data to generate point clouds
vector<KeyFrame*> keyframes;
vector<cv::Mat> colorImgs;
vector<cv::Mat> depthImgs;
mutex keyframeMutex;
uint16_t lastKeyframeSize =0;
double resolution = 0.04;
pcl::VoxelGrid<PointT> voxel;
};
#endif // POINTCLOUDMAPPING_H
include/System.h
// for point cloud viewing
#include "pointcloudmapping.h"
class PointCloudMapping;
private:
// point cloud mapping
shared_ptr<PointCloudMapping> mpPointCloudMapping;
include/Tracking.h
// for pointcloud mapping and viewing
#include "pointcloudmapping.h"
class PointCloudMapping;
public:
//Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
//KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, shared_ptr<PointCloudMapping> pPointCloud,
KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
cv::Mat mlmrgb;//增加此处
cv::Mat mImDepth;
protected:
// for point cloud viewing
shared_ptr<PointCloudMapping> mpPointCloudMapping;
新建如下文件
可以直接从ORBSLAM2_with_pointcloud_map复制
src/pointcloudmapping.cc
#include "pointcloudmapping.h"
#include <KeyFrame.h>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/projection_matrix.h>
#include "Converter.h"
#include <boost/make_shared.hpp>
PointCloudMapping::PointCloudMapping(double resolution_)
{
this->resolution = resolution_;
voxel.setLeafSize( resolution, resolution, resolution);
globalMap = boost::make_shared< PointCloud >( );
viewerThread = make_shared<thread>( bind(&PointCloudMapping::viewer, this ) );
}
void PointCloudMapping::shutdown()
{
{
unique_lock<mutex> 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 = "<<kf->mnId<<endl;
unique_lock<mutex> lck(keyframeMutex);
keyframes.push_back( kf );
colorImgs.push_back( color.clone() );
depthImgs.push_back( depth.clone() );
keyFrameUpdated.notify_one();
}
void PointCloudMapping::save()
{
pcl::io::savePCDFile( "result.pcd", *globalMap );
cout<<"globalMap save finished"<<endl;
}
pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud(KeyFrame* kf, cv::Mat& color, cv::Mat& depth)
{
PointCloud::Ptr tmp( new PointCloud() );
// cv::imshow("123",color);
// cv::waitKey(1);
// 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);
}
}
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 "<<kf->mnId<<", size="<<cloud->points.size()<<endl;
return cloud;
}
void PointCloudMapping::viewer()
{
pcl::visualization::CloudViewer viewer("viewer");
while(1)
{
{
unique_lock<mutex> lck_shutdown( shutDownMutex );
if (shutDownFlag)
{
break;
}
}
{
unique_lock<mutex> lck_keyframeUpdated( keyFrameUpdateMutex );
keyFrameUpdated.wait( lck_keyframeUpdated );
}
// keyframe is updated
size_t N=0;
{
unique_lock<mutex> lck( keyframeMutex );
N = keyframes.size();
}
for ( size_t i=lastKeyframeSize; i<N ; i++ )
{
PointCloud::Ptr p = generatePointCloud( keyframes[i], colorImgs[i], depthImgs[i] );
*globalMap += *p;
}
PointCloud::Ptr tmp(new PointCloud());
voxel.setInputCloud( globalMap );
voxel.filter( *tmp );
globalMap->swap( *tmp );
viewer.showCloud( globalMap );
cout << "show global map, size=" << globalMap->points.size() << endl;
lastKeyframeSize = N;
}
}
src/System.cc
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer):
mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
mbDeactivateLocalizationMode(false)
{ //Check settings file
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
cerr << "Failed to open settings file at: " << strSettingsFile << endl;
exit(-1);
}
// for point cloud resolution
float resolution = fsSettings["PointCloudMapping.Resolution"];//增加此处
---------------------------------------------------------------------
//Create Drawers. These are used by the Viewer
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
// Initialize pointcloud mapping
mpPointCloudMapping = make_shared<PointCloudMapping>( resolution );//增加此处
//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, mpPointCloudMapping, mpKeyFrameDatabase, strSettingsFile, mSensor);//增加此处
}
void System::Shutdown()
{
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
mpPointCloudMapping->shutdown();//增加此处
}
void System::save()
{
mpPointCloudMapping->save();
}
src/Tracking.cc
Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, shared_ptr<PointCloudMapping> pPointCloud, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):
mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
mpPointCloudMapping( pPointCloud ),
mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),
mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)
void Tracking::CreateNewKeyFrame()
{
mpLocalMapper->SetNotStop(false);
// insert Key Frame into point cloud viewer
mpPointCloudMapping->insertKeyFrame( pKF, this->mlmrgb, this->mImDepth );//增加此处
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
}
检查下方函数写法,
如果有cv::Mat imDepth = imD;
则要改写,替换成mImDepth
否则,建图结果全是散的
cv::Mat Tracking::GrabImageRGBD(
const cv::Mat &imRGB, //彩色图像
const cv::Mat &imD, //深度图像
const double ×tamp, //时间戳
const vector<cv::Mat> &mask_solov2) //solov2遮罩
{
mlmrgb=imRGB; //增加此处
mImGray = imRGB;
mImDepth= imD;
// cv::Mat imDepth = imD;
// step 1:将RGB或RGBA图像转为灰度图像
if(mImGray.channels()==3)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
else if(mImGray.channels()==4)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
// step 2 :将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度
//这里的判断条件感觉有些尴尬
//前者和后者满足一个就可以了
//满足前者意味着,mDepthMapFactor 相对1来讲要足够大
//满足后者意味着,如果深度图像不是浮点型? 才会执行
//意思就是说,如果读取到的深度图像是浮点型,就不执行这个尺度的变换操作了呗? TODO
if((fabs(mDepthMapFactor-1.0f)>1e-5) || mImDepth.type()!=CV_32F)
mImDepth.convertTo( //将图像转换成为另外一种数据类型,具有可选的数据大小缩放系数
mImDepth, //输出图像
CV_32F, //输出图像的数据类型
mDepthMapFactor); //缩放系数
// 步骤3:构造Frame
mCurrentFrame = Frame(
mImGray, //灰度图像
mImDepth, //深度图像
timestamp, //时间戳
mask_solov2, //solov2遮罩
mpORBextractorLeft, //ORB特征提取器
mpORBVocabulary, //词典
mK, //相机内参矩阵
mDistCoef, //相机的去畸变参数
mbf, //相机基线*相机焦距
mThDepth); //内外点区分深度阈值
// 步骤4:跟踪
Track();
//返回当前帧的位姿
return mCurrentFrame.mTcw.clone();
}