基于reslsense d435相机的ORBSLAM2/3非ros下实时建图
orbslam2/3非ros下用d435数据建图
因需求需要在ubuntu系统下不使用ros进行orbslam2的使用，官方源码在非ros下只能使用数据集输入。
源码改动
只改动了/home/xxx/orbslam_ws/src/ORB_SLAM2/CMakeLists.txt和/home/dong/orbslam_ws/src/ORB_SLAM2/Examples/RGB-D/rgbd_tum.cc
CMakeLists.txt
在其中find_package(XXXXXXX REQUIRED)下加入以下代码，其中相关兼容库请自行下载，相关路径也自行更改
FIND_PACKAGE(realsense2 REQUIRED)
find_package(GLUT REQUIRED)
include_directories(${GLUT_INCLUDE_DIRS})
link_directories(${GLUT_LIBRARY_DIRS})
add_definitions(${GLUT_DEFINITIONS})
if(NOT GLUT_FOUND)
message(ERROR " GLUT not found!")
endif(NOT GLUT_FOUND)
#########################################################
# FIND OPENGL
#########################################################
find_package(OpenGL REQUIRED)
find_package(glfw3 REQUIRED)
find_package(PCL REQUIRED)
find_package(Pangolin REQUIRED)
include_directories(${OpenGL_INCLUDE_DIRS})
link_directories(${OpenGL_LIBRARY_DIRS})
add_definitions(${OpenGL_DEFINITIONS})
if(NOT OPENGL_FOUND)
message(ERROR " OPENGL not found!")
endif(NOT OPENGL_FOUND)
include_directories(/home/xxx/librealsense-2.36.0/include)
set(DEPENDENCIES realsense2 )
include_directories(/home/xxx/realsense/include)
在target_link_libraries(rgbd_tum ${PROJECT_NAME} ${OPENGL_LIBRARIES} ${realsense2_LIBRARY})加入 gl和rs2库
rgbd_tum.cc
源码整个替换
#include<iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include<librealsense2/rsutil.h>
#include <librealsense2/rs.hpp>
//#include "/home/dong/librealsense-2.36.0/examples/example.hpp"
// include OpenCV header file
// Helper functions
#include <unistd.h>
#include<opencv2/core/core.hpp>
#include <opencv2/core/mat.hpp>
#include <sys/time.h>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<System.h>
#define Width 640
#define Height 480
#define fps 30
using namespace std;
using namespace cv;
using namespace rs2;
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps);
float get_depth_scale(device dev)
{
for (sensor& sensor : dev.query_sensors()) //检查设备的传感器
{
if (depth_sensor dpt = sensor.as<depth_sensor>()) //检查传感器是否为深度传感器
{
return dpt.get_depth_scale();
}
}
throw runtime_error("Device does not have a depth sensor");
}
//检查摄像头数据管道设置是否改变
bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev)
{
for (auto&& sp : prev)
{
//If previous profile is in current (maybe just added another)
auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile& current_sp) { return sp.unique_id() == current_sp.unique_id(); });
if (itr == std::end(current)) //If it previous stream wasn't found in current
{
return true;
}
}
return false;
}
int main(int argc, char **argv)
{
/*if(argc != 5)
{
cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
return 1;
}*/
// Retrieve paths to images
vector<string> vstrImageFilenamesRGB;
vector<string> vstrImageFilenamesD;
vector<double> vTimestamps;
//string strAssociationFilename = string(argv[4]);
//LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);
// Check consistency in the number of images and depthmaps
int nImages = vstrImageFilenamesRGB.size();
/*if(vstrImageFilenamesRGB.empty())
{
cerr << endl << "No images found in provided path." << endl;
return 1;
}
else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size())
{
cerr << endl << "Different number of images for rgb and depth." << endl;
return 1;
}
*/
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
// Vector for tracking time statistics
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;
// Main loop
struct timeval tv;
cv::Mat dep;
rs2::context ctx;
auto list = ctx.query_devices(); // Get a snapshot of currently connected devices
if (list.size() == 0)
throw std::runtime_error("no camera");
rs2::device dev = list.front();
rs2::frameset frames;
//Contruct a pipeline which abstracts the device
//深度图像颜色map
rs2::colorizer c; // Helper to colorize depth images
//helper用于渲染图片
//texture renderer; // Helper for renderig images
// Create a pipeline to easily configure and start the camera
//创建数据管道
rs2::pipeline pipe;
rs2::config pipe_config;
pipe_config.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
pipe_config.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);
//Calling pipeline's start() without any additional parameters will start the first device
//直接start()，不添加配置参数，则默认打开第一个设备
// with its default streams.
//以及以默认的配置进行流输出
//The start function returns the pipeline profile which the pipeline used to start the device
//start()函数返回数据管道的profile
rs2::pipeline_profile profile = pipe.start(pipe_config);
// Each depth camera might have different units for depth pixels, so we get it here
//每个深度摄像头有不同单元的像素，我们这里获取
// Using the pipeline's profile, we can retrieve the device that the pipeline uses
//使用数据管道的profile获取深度图像像素对应于长度单位（米）的转换比例
float depth_scale = get_depth_scale(profile.get_device());
//Pipeline could choose a device that does not have a color stream
//数据管道可以选择一个没有彩色图像数据流的设备
//If there is no color stream, choose to align depth to another stream
//选择彩色图像数据流来作为对齐对象
rs2_stream align_to = RS2_STREAM_COLOR;//find_stream_to_align(profile.get_stream());
/*
@这里的对齐是改变深度图，而不改变color图
*/
// Create a rs2::align object.
//创建一个rs2::align的对象
// rs2::align allows us to perform alignment of depth frames to others frames
//rs2::align 允许我们去实现深度图像对齐其他图像
//The "align_to" is the stream type to which we plan to align depth frames.
// "align_to"是我们打算用深度图像对齐的图像流
rs2::align align(align_to);
// Define a variable for controlling the distance to clip
//定义一个变量去转换深度到距离
float depth_clipping_distance = 1.f;
int i=0;
while (true)
{
i++;
rs2::frameset frameset = pipe.wait_for_frames();
// rs2::pipeline::wait_for_frames() can replace the device it uses in case of device error or disconnection.
// Since rs2::align is aligning depth to some other stream, we need to make sure that the stream was not changed
//因为rs2::align 正在对齐深度图像到其他图像流，我们要确保对齐的图像流不发生改变
// after the call to wait_for_frames();
if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
{
//If the profile was changed, update the align object, and also get the new device's depth scale
//如果profile发生改变，则更新align对象，重新获取深度图像像素到长度单位的转换比例
profile = pipe.get_active_profile();
align = rs2::align(align_to);
depth_scale = get_depth_scale(profile.get_device());
}
//Get processed aligned frame
//获取对齐后的帧
auto processed = align.process(frameset);
// Trying to get both other and aligned depth frames
//尝试获取对齐后的深度图像帧和其他帧
rs2::frame aligned_color_frame = processed.get_color_frame();//processed.first(align_to);
rs2::frame aligned_depth_frame = processed.get_depth_frame();
//获取对齐之前的color图像
rs2::frame before_depth_frame=frameset.get_depth_frame();
//获取宽高
const int depth_w=aligned_depth_frame.as<rs2::video_frame>().get_width();
const int depth_h=aligned_depth_frame.as<rs2::video_frame>().get_height();
const int color_w=aligned_color_frame.as<rs2::video_frame>().get_width();
const int color_h=aligned_color_frame.as<rs2::video_frame>().get_height();
const int b_color_w=before_depth_frame.as<rs2::video_frame>().get_width();
const int b_color_h=before_depth_frame.as<rs2::video_frame>().get_height();
//If one of them is unavailable, continue iteration
if (!aligned_depth_frame || !aligned_color_frame)
{
continue;
}
//创建OPENCV类型 并传入数据
Mat imD(Size(depth_w,depth_h),CV_16U,(void*)aligned_depth_frame.get_data(),Mat::AUTO_STEP);
Mat imRGB(Size(color_w,color_h),CV_8UC3,(void*)aligned_color_frame.get_data(),Mat::AUTO_STEP);
Mat before_color_image(Size(b_color_w,b_color_h),CV_16U,(void*)before_depth_frame.get_data(),Mat::AUTO_STEP);
gettimeofday(&tv, NULL);
double tframe = tv.tv_sec%100000+tv.tv_usec*0.000001;
/* for(int ni=0; ni<nImages; ni++)
{
// Read image and depthmap from file
imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);
double tframe = vTimestamps[ni];
if(imRGB.empty())
{
cerr << endl << "Failed to load image at: "
<< string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;
return 1;
}*/
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif
// Pass the image to the SLAM system
SLAM.TrackRGBD(imRGB,imD,tframe);
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
/* double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack;
// Wait to load the next frame
double T=0;
if(ni<nImages-1)
T = vTimestamps[ni+1]-tframe;
else if(ni>0)
T = tframe-vTimestamps[ni-1];
if(ttrack<T)
usleep((T-ttrack)*1e6);
}*/
}
// Stop all threads
SLAM.Shutdown();
// Tracking time statistics
/*sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
for(int ni=0; ni<nImages; ni++)
{
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;
*/
// Save camera trajectory
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
return 0;
}
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
ifstream fAssociation;
fAssociation.open(strAssociationFilename.c_str());
while(!fAssociation.eof())
{
string s;
getline(fAssociation,s);
if(!s.empty())
{
stringstream ss;
ss << s;
double t;
string sRGB, sD;
ss >> t;
vTimestamps.push_back(t);
ss >> sRGB;
vstrImageFilenamesRGB.push_back(sRGB);
ss >> t;
ss >> sD;
vstrImageFilenamesD.push_back(sD);
}
}
}
运行
./Examples/RGB-D/rgbd_tum ./Vocabulary/ORBvoc.txt ./Examples/RGB-D/TUM1.yaml