基于RGB-D的视觉里程计
参考高博:一起做RGBD-SLAM
相关代码:放在本人github,还在调整
自己跟着写了一遍,做了更多的注释
文件结构
外层CmakeLists.txt
#设定存放编译出来的库文件的目录
SET( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#并且把该目录设为连接目录
LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib)
#设定头文件目录
INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include)
#增加子文件夹,也就是进入源代码文件夹继续构建
ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src)
单帧图像点云表示 imageToCloud.cpp
// C++ 标准库
#include <iostream>
#include <string>
using namespace std;
// OpenCV 库
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// PCL 库
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 相机内参
const double camera_factor = 1000;
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;
// 主函数
int main( int argc, char** argv )
{
// 读取./data/rgb.png和./data/depth.png,并转化为点云
// 图像矩阵
cv::Mat rgb, depth;
// 使用cv::imread()来读取图像
// rgb 图像是8UC3的彩色图像
rgb = cv::imread( "./data/rgb.png" );
// depth 是16UC1的单通道图像,注意flags设置-1,表示读取原始数据不做任何修改
depth = cv::imread( "./data/depth.png", -1 );
// 点云变量
// 使用智能指针,创建一个空点云。这种指针用完会自动释放。
PointCloud::Ptr cloud ( new PointCloud );
// 遍历深度图
for (int m = 0; m < depth.rows; m++)
for (int n=0; n < depth.cols; n++)
{
// 获取深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d 可能没有值,若如此,跳过此点
if (d == 0)
continue;
// d 存在值,则向点云增加一个点
PointT p;
// 计算这个点的空间坐标
p.z = double(d) / camera_factor;
p.x = (n - camera_cx) * p.z / camera_fx;
p.y = (m - camera_cy) * p.z / camera_fy;
// 从rgb图像中获取它的颜色
// rgb是三通道的BGR格式图,所以按下面的顺序获取颜色
p.b = rgb.ptr<uchar>(m)[n*3];
p.g = rgb.ptr<uchar>(m)[n*3+1];
p.r = rgb.ptr<uchar>(m)[n*3+2];
// 把p加入到点云中
cloud->points.push_back( p );
}
// 设置并保存点云
cloud->height = 1;
cloud->width = cloud->points.size();
cout<<"point cloud size = "<<cloud->points.size()<<endl;
cloud->is_dense = false;
pcl::io::savePCDFile( "./pointcloud.pcd", *cloud );
// 清除数据并退出
cloud->points.clear();
cout<<"Point cloud saved."<<endl;
return 0;
}
可以实现功能,但是太混乱了,根据这个函数拓展开,下面的函数封装才是正题
正题:函数封装slamBase.h
CMakeLists.txt
# 增加PCL库的依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization)
# 增加opencv的依赖
FIND_PACKAGE( OpenCV REQUIRED )
# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS} )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
#将写好的库文件链接
ADD_LIBRARY( slambase slamBase.cpp )
TARGET_LINK_LIBRARIES( slambase
${OpenCV_LIBS}
${PCL_LIBRARIES} )
#生成可执行文件
ADD_EXECUTABLE( joinPointCloud joinPointCloud.cpp)
#可执行文件链接相应的库文件
TARGET_LINK_LIBRARIES( joinPointCloud ${OpenCV_LIBS}
${PCL_LIBRARIES}
slambase)
0、参数读取ParameterReader
//定义一个管理配置文件的类
// 参数读取类
class ParameterReader
{
public:
//构造函数,读取文件的相对路径
ParameterReader( string filename="./config/parameters.txt" )
{
ifstream fin( filename.c_str() ); //读取文件
if (!fin)
{
cerr<<"parameter file does not exist."<<endl;
return;
}
while(!fin.eof()) //end of file?
{
string str;
getline( fin, str );
if (str[0] == '#')
{
// 以‘#’开头的是注释
continue;
}
int pos = str.find("="); //find 找到返回位置,其余为false
if (pos == -1)
continue;
string key = str.substr( 0, pos ); //复制子字符串,要求从指定位置开始,并具有指定的长度。如果没有指定长度_Count或_Count+_Off超出了源字符串的长度,则子字符串将延续到源字符串的结尾。
string value = str.substr( pos+1, str.length() );
data[key] = value;
if ( !fin.good() ) //fin.good是判断文件是否打开的,如果返回真的话就是打开了,否则没有打开
break;
}
}
string getData( string key )
{
map<string, string>::iterator iter = data.find(key);
if (iter == data.end())
{
cerr<<"Parameter name "<<key<<" not found!"<<endl;
return string("NOT_FOUND");
}
return iter->second;
}
public:
map<string, string> data;
};
1、相机内参数结构体CAMERA_INTRINSIC_PARAMETERS
// 相机内参结构,使用结构体的形式,更好的管理参数类文件,可以经常使用
struct CAMERA_INTRINSIC_PARAMETERS {
double cx, cy, fx, fy, scale/*缩放尺度*/;
};
2、Frame结构体
每帧图像包含了很多信息,封装为结构体会结构会很清晰
// 帧结构
struct FRAME
{
int frameID;
cv::Mat rgb, depth; //该帧对应的彩色图与深度图
cv::Mat desp; //特征描述子
vector<cv::KeyPoint> kp; //关键点
};
3、Pnp返回的结果结构体RESULT_OF_PNP
// PnP 结果
struct RESULT_OF_PNP
{
cv::Mat rvec/*旋转向量,让点云旋转时使用的时候要转换为Eigen格式*/, tvec/*平移向量*/;
int inliers; //内点(就是自己定义的规则觉得比较好的点)数量
};
4、2D点转3D点point2dTo3d
由于需要用到pnp来求解位姿,所以需要一个3D点,将带深度的图像点(u,v,d)经过
相机内参转到相机坐标系下
/**
* @brief 将单个点从图像坐标转换为空间坐标
* @param[in] point 图像点(u,v)+一个相对深度d ----> (u,v,d)
* @param[in] camera 参数
* @return cv::Point3f
*/
cv::Point3f point2dTo3d(cv::Point3f &point, CAMERA_INTRINSIC_PARAMETERS &camera)
{
//初始化输出3d点的类型,以及为其开辟栈区内存空间
cv::Point3f _p;
//相机投影的公式
//相机投影公式出问题了,少除了个camera.scale
//_p.z = double(point.z);
_p.z = double(point.z) / camera.scale;
_p.x = (point.x - camera.cx) * _p.z / camera.fx;
_p.y = (point.y - camera.cy) * _p.z / camera.fy;
return _p;
}
5、图像转点云函数image2PointCloud
/* *
* @brief image2PointCloud函数,用于2D图片到3D点云图片的转换
* @param[in] rgb图片、深度图片、相机内参数
* @param[out] 点云智能指针,指向点云的一个空间(栈区)
* @return 0; 成功执行
* @return -1; 读取图像失败,
*/
PointCloud::Ptr image2PointCloud(cv::Mat &rgb, cv::Mat &depth, CAMERA_INTRINSIC_PARAMETERS &camera)
{
// 使用智能指针,创建一个空点云。这种指针用完会自动释放。
PointCloud::Ptr cloud(new PointCloud);
// 遍历深度图
for (int m = 0; m < depth.rows; m++)
for (int n = 0; n < depth.cols; n++) {
// 获取深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d 可能没有值,若如此,跳过此点
if (d == 0)
continue;
// d 存在值,则向点云增加一个点
PointT p;
// 计算这个点的空间坐标
p.z = double(d) / camera.scale;
p.x = (n - camera.cx) * p.z / camera.fx;
p.y = (m - camera.cy) * p.z / camera.fy;
// 从rgb图像中获取它的颜色
// rgb是三通道的BGR格式图,所以按下面的顺序获取颜色
p.b = rgb.ptr<uchar>(m)[n * 3];
p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
// 把p加入到点云中
cloud->points.push_back(p);
}
// 设置并保存点云
cloud->height = 1;
cloud->width = cloud->points.size();
cout << "point cloud size = " << cloud->points.size() << endl;
cloud->is_dense = false;
//pcl::io::savePCDFile("./pointcloud.pcd", *cloud);
// 清除数据并退出
//cloud->points.clear();
//cout << "Point cloud saved." << endl;
return cloud;
}
6、特征点提取与描述子的计算(ORB)computeKeyPointsAndDesp
/**
* @brief 计算关键点和描述子
* @param[in] frame frame传入时应该具有rgb和depth信息,传出时有kp和descriptor数据
*/
void computeKeyPointsAndDesp(FRAME &frame)
{
// 构建提取器,默认两者都为 ORB
cv::Ptr<cv::FeatureDetector> _detector = cv::ORB::create();
cv::Ptr<cv::DescriptorExtractor> _descriptor = cv::ORB::create();
_detector->detect(frame.rgb, frame.kp); //提取图像1关键点
if (!frame.kp.data())
{
cout << "关键点提取失败,没有内容,Err() \n";
return;
}
cout << "Key points of image: " << frame.kp.size() << endl;
// 计算描述子
_descriptor->compute(frame.rgb, frame.kp, frame.desp);
}
7、Pnp estimateMotion
/**
* @brief pnp: 3d - 2d 关系求解位姿变换
* @param[in] frame1 帧1
* @param[in] frame2 帧2
* @param[in] camera 相机内参
* @return RESULT_OF_PNP pnp的结果,就是一个r 一个t,一定要分清楚,他们的坐标关系!
*/
RESULT_OF_PNP estimateMotion(FRAME &frame1, FRAME &frame2, CAMERA_INTRINSIC_PARAMETERS &camera)
{
// 求解pnp
RESULT_OF_PNP _res;
// 匹配描述子
vector<cv::DMatch> _matches;
//用BruteForce-Hamming来计算匹配关系
cv::Ptr<cv::DescriptorMatcher> _matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
//cv::BFMatcher matcher;
_matcher->match(frame1.desp, frame2.desp, _matches);
if (_matches.data() == 0) {
cout << "matches里面没有内容" << endl;
}
cout << "Find total " << _matches.size() << " matches." << endl;
vector<cv::DMatch> _goodMatches;
double minDis = 9999; //这个9999哪来的?傻逼,这是随便取的,取大点就可以了
for (size_t i = 0; i < _matches.size(); i++)
{
if (_matches[i].distance < minDis)
{
minDis = _matches[i].distance;
}
}
cout << "min dis = " << minDis << endl;
for (size_t i = 0; i < _matches.size(); i++)
{
if (_matches[i].distance < 10 * minDis)
{
_goodMatches.push_back(_matches[i]);
}
}
cout<<"good matches: "<<_goodMatches.size()<<endl;
//goodmatch较少的处理方式
if (_goodMatches.size() <= 5)
{
_res.inliers = -1;
return _res;
}
// 第一个帧的三维点
vector<cv::Point3f> _pts_obj;
// 第二个帧的图像点
vector<cv::Point2f> _pts_img;
for (size_t i = 0; i < _goodMatches.size(); i++)
{
// query 是第一个, train 是第二个
cv::Point2f _p = frame1.kp[_goodMatches[i].queryIdx].pt;
// 获取d是要小心!x是向右的,y是向下的,所以y才是行,x是列!
ushort _d = frame1.depth.ptr<ushort>(int(_p.y))[int(_p.x)];
if (_d == 0)
{
continue;
}
_pts_img.push_back(cv::Point2f(frame2.kp[_goodMatches[i].trainIdx].pt));
// 将图像点(u,v,d)转成相机帧下的(x,y,z)
cv::Point3f _pt(_p.x, _p.y, _d);
cv::Point3f _pd = point2dTo3d(_pt, camera); //经过内参变化
_pts_obj.push_back(_pd);
}
double _camera_matrix_data[3][3] = {
{camera.fx, 0, camera.cx},
{0, camera.fy, camera.cy},
{0, 0, 1}
};
// 构建相机矩阵
cv::Mat _cameraMatrix(3, 3, CV_64F, _camera_matrix_data);
cv::Mat _inliers;
//brings points from the model coordinate system to the camera coordinate system.
//把3d点从自身坐标转化到图像坐标系下
cv::solvePnPRansac(_pts_obj, _pts_img, _cameraMatrix, cv::Mat(), _res.rvec, _res.tvec, -1, 100, 8.0, 0.99, _inliers);
_res.inliers = _inliers.rows;
cout << "inliers: " << _res.inliers << endl;
cout << "R=" << _res.rvec << endl;
cout << "t=" << _res.tvec << endl;
return _res;
}
8、新加入的帧转换为点云joinPointCloud
/**
* @brief 将新加入的帧转换为点云,然后将原始点云旋转到新加入点云的坐标系下,叠加后输出
* @param[in] original 原始点云
* @param[in] newFrame 新来的帧
* @param[in] T T为上一帧到当前帧的变换
* @param[in] camera 相机内参
* @return 将新来帧加到原始帧后的点云图像
*/
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera )
{
//新加入的帧转换为点云
PointCloud::Ptr _newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera );
// 合并点云
PointCloud::Ptr _output (new PointCloud()); //定义一个输出点云
pcl::transformPointCloud( *original, *_output, T.matrix() ); //将原始点云通过T(T为上一帧到当前帧的变换)转换到新加入的点云坐标系下
*_newCloud += *_output; //简单叠加
return _newCloud;
}
main函数实现,两帧图像构建点云
#include<iostream>
#include "slamBase.h"
#include <opencv2/core/eigen.hpp>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
// Eigen !
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
int main( int argc, char** argv )
{
//本节要拼合data中的两对图像
myslam::ParameterReader pd; //参数读取器的实例化对象
// 声明两个帧,FRAME结构请见include/slamBase.h
myslam::FRAME frame1, frame2; //用帧结构体创建两帧图片
//读取图像
frame1.rgb = cv::imread( "./data/rgb1.png" );
frame1.depth = cv::imread( "./data/depth1.png", -1);
frame2.rgb = cv::imread( "./data/rgb2.png" );
frame2.depth = cv::imread( "./data/depth2.png", -1 );
//判断读取的图像是否正常
if(frame1.rgb.data == 0 || frame2.rgb.data == 0
|| frame1.depth.data == 0 || frame2.depth.data == 0)
{
cout << "Load pictures errs...." << endl;
return -1;
}
// 提取特征并计算描述子
cout<<"extracting features"<<endl;
computeKeyPointsAndDesp(frame1); //计算frame1描述子,默认的ORB,可以进函数改
computeKeyPointsAndDesp(frame2);
// 相机内参
myslam::CAMERA_INTRINSIC_PARAMETERS camera;
camera.fx = atof( pd.getData( "camera.fx" ).c_str()); //atof:Convert a string to a floating-point number
camera.fy = atof( pd.getData( "camera.fy" ).c_str());
camera.cx = atof( pd.getData( "camera.cx" ).c_str());
camera.cy = atof( pd.getData( "camera.cy" ).c_str());
camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
// 求解pnp
//求出来的是第一帧(Frame1)三维点在第一帧的坐标系下,经过怎样的变换(result)到第二帧的坐标系下
cout<<"solving pnp"<<endl;
//自己封装的函数::利用PNP求解相对位姿,输入两帧的结构,相机内参;返回RESULT_OF_PNP结构体
myslam::RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );
cout<<result.rvec<<endl<<result.tvec<<endl;
// 处理result
// 将旋转向量转化为旋转矩阵
cv::Mat R;
cv::Rodrigues( result.rvec, R ); //罗德里格斯公式,转换为旋转矩阵R
cout << "罗德里格斯公式转换后的R = \n" << R << endl;
Eigen::Matrix3d r;
cv::cv2eigen(R, r); //转换为Eigen形式
// 将平移向量和旋转矩阵转换成变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd angle(r); //构造一个旋转向量
cout << "angle = \n" << angle.matrix() << endl; //angle.matrix() 和 r 相等
cout<<"translation"<<endl;
//T = angle;
T.rotate(angle); //构造变换矩阵T-----参考《视觉SLAM14讲》p63
T(0,3) = result.tvec.at<double>(0,0);
T(1,3) = result.tvec.at<double>(0,1);
T(2,3) = result.tvec.at<double>(0,2);
cout << "变换矩阵 T = \n" << T.matrix() << endl;
// 转换点云
cout<<"converting image to clouds"<<endl;
//将rgb和depth转化为点云
myslam::PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
// 合并点云
cout<<"combining clouds"<<endl;
myslam::PointCloud::Ptr output (new myslam::PointCloud());
//这里的意思为:T左乘 *cloud1 变换到第二帧坐标系下
output = myslam::joinPointCloud( cloud1, frame2 , T, camera );
pcl::io::savePCDFile("data/result.pcd", *output);
cout<<"Final result saved."<<endl;
//显示点云
pcl::visualization::CloudViewer viewer( "viewer" );
viewer.showCloud( output );
while( !viewer.wasStopped() )
{
}
return 0;
}