注：本篇文章只是对高翔博士稠密点云代码进行的简述，内容主要包括的是在ORB-SLAM2基础上怎么添加稠密建图线程，并未对高翔博士代码进行改动。本文章仅用于自己学习记录，若有侵权麻烦私聊联系删除。
目录
一、添加pointcloudMapping类
二、对源代码进行修改
三、修改CMakeLists.txt
四、添加文件bin_vocabulary.cc
代码参考：
https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map
一、添加pointcloudMapping类
在原ORB-SLAM2源码中添加pointcloudMapping类，在类内构造函数中添加了稠密点云线程；在Tracking过程中将关键帧的RGB图像和深度图像加入到稠密点云线程内。
pointcloudmapping.cc
/*
* <one line to give the program's name and a brief idea of what it does.>
* Copyright (C) 2016 <copyright holder> <email>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#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 <pcl/io/pcd_io.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();
}
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);
}
}
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;
}
pcl::io::savePCDFileBinary("vslam.pcd", *globalMap); // new
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;
}
}
pointcloudmapping.h
/*
* <one line to give the program's name and a brief idea of what it does.>
* Copyright (C) 2016 <copyright holder> <email>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#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>
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();
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
二、对源代码进行修改
添加头文件和点云类
//System.h
#include "pointcloudmapping.h"
class PointCloudMapping;
//Tracking.h
#include "pointcloudmapping.h"
class PointCloudMapping;
添加成员变量
//System.h
std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
std::mutex mMutexState;
//***
shared_ptr<PointCloudMapping> mpPointCloudMapping;
//Tracking.h
//Color order (true RGB, false BGR, ignored if grayscale)
bool mbRGB;
list<MapPoint*> mlpTemporalPoints;
//***
shared_ptr<PointCloudMapping> mpPointCloudMapping;
创建PointCloudMapping对象，在初始化Tracking线程的时候传入
//System.cc
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
//***
mpPointCloudMapping = make_shared<PointCloudMapping> (resolution);
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,mpMap, mpPointCloudMapping,mpKeyFrameDatabase, strSettingsFile, mSensor);
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
对变量resolution进行定义，设定初值实在相机配件文件中，例如TUM3.yaml
if(!fsSettings.isOpened())
{
cerr << "Failed to open settings file at: " << strSettingsFile << endl;
exit(-1);
}
//***
float resolution = fsSettings["PointCloudMapping.Resolution"];
在Tracking.cc和Tracking.h文件中对Tracking的构造函数进行修改，添加参数shared_ptr<PointCloudMapping> pPoingCloud
//Tracking.h
//***
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,shared_ptr<PointCloudMapping> pPoingCloud,KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
//Tracking.cc
//***
Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, shared_ptr<PointCloudMapping> pPoingCloud,KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),mpPointCloudMapping(pPoingCloud),mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)
在Tracking.cc中，对点云线程插入关键帧
//Tracking.cc
mpLocalMapper->InsertKeyFrame(pKF);
mpLocalMapper->SetNotStop(false);
//***
//mpPointCloudMapping->insertKeyFrame(pKF,this->mImGray,this->mImDepth);
mpPointCloudMapping->insertKeyFrame(pKF,this->mImRGB,this->imDepth);
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
对mpPointCloudMapping->insertKeyFrame()函数参数进行定义
//Tracking.cc
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp)
{
//***
mImGray = imRGB;
imDepth = imD;
mImRGB = imRGB;
//Tracking.h
// Current Frame
Frame mCurrentFrame;
cv::Mat mImGray;
//***
cv::Mat mImRGB;
cv::Mat imDepth;
在System.cc中void System::Shutdown()函数中调用关闭稠密点云线程函数
System.cc
void System::Shutdown()
{
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
//***
mpPointCloudMapping->shutdown();
三、修改CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(ORB_SLAM2)
SET(CMAKE_EXPORT_COMPILE_COMMANDS "ON")
IF(NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE Release)
ENDIF()
MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 ")
# Check C++11 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
find_package(OpenCV 3.0 QUIET)
if(NOT OpenCV_FOUND)
find_package(OpenCV 2.4.3 QUIET)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
endif()
endif()
find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
# 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} )
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
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}
)
# Build examples
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
add_executable(rgbd_tum
Examples/RGB-D/rgbd_tum.cc)
target_link_libraries(rgbd_tum ${PROJECT_NAME})
add_executable(stereo_kitti
Examples/Stereo/stereo_kitti.cc)
target_link_libraries(stereo_kitti ${PROJECT_NAME})
add_executable(stereo_euroc
Examples/Stereo/stereo_euroc.cc)
target_link_libraries(stereo_euroc ${PROJECT_NAME})
add_executable(mono_tum
Examples/Monocular/mono_tum.cc)
target_link_libraries(mono_tum ${PROJECT_NAME})
add_executable(mono_kitti
Examples/Monocular/mono_kitti.cc)
target_link_libraries(mono_kitti ${PROJECT_NAME})
add_executable(mono_euroc
Examples/Monocular/mono_euroc.cc)
target_link_libraries(mono_euroc ${PROJECT_NAME})
# Build tools
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/tools)
add_executable(bin_vocabulary
tools/bin_vocabulary.cc)
target_link_libraries(bin_vocabulary ${PROJECT_NAME})
四、添加文件bin_vocabulary.cc
暂不知道具体什么作用，看文件应该是与词典有关，用来回环用的。（注意CMakeLists.txt文件中对bin_vocabulary.cc编译的路径）
#include <time.h>
#include "ORBVocabulary.h"
using namespace std;
bool load_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string infile);
void load_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string infile);
void load_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string infile);
void save_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile);
void save_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile);
void save_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile);
int main(int argc, char **argv) {
cout << "BoW load/save benchmark" << endl;
ORB_SLAM2::ORBVocabulary* voc = new ORB_SLAM2::ORBVocabulary();
load_as_text(voc, "Vocabulary/ORBvoc.txt");
save_as_binary(voc, "Vocabulary/ORBvoc.bin");
return 0;
}
bool load_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
clock_t tStart = clock();
bool res = voc->loadFromTextFile(infile);
printf("Loading fom text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
return res;
}
void load_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
clock_t tStart = clock();
voc->load(infile);
printf("Loading fom xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}
void load_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
clock_t tStart = clock();
voc->loadFromTextFile(infile);
printf("Loading fom binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}
void save_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
clock_t tStart = clock();
voc->saveToTextFile(outfile);
printf("Saving as text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}
void save_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
clock_t tStart = clock();
voc->save(outfile);
printf("Saving as xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}
void save_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
clock_t tStart = clock();
voc->saveToTextFile(outfile);
printf("Saving as binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
