第四讲 点云拼接

  广告:“一起做”系列的代码网址:https://github.com/gaoxiang12/rgbd-slam-tutorial-gx 当博客更新时代码也会随着更新。 SLAM技术交流群:374238181

  读者朋友们大家好!尽管还没到一周,我们的教程又继续更新了,因为暑假实在太闲了嘛!


上讲回顾

  上一讲中,我们理解了如何利用图像中的特征点,估计相机的运动。最后,我们得到了一个旋转向量与平移向量。那么读者可能会问:这两个向量有什么用呢?在这一讲里,我们就要使用这两个向量,把两张图像的点云给拼接起来,形成更大的点云。

  首先,我们把上一讲的内容封装进slamBase库中,代码如下:

  include/slamBase.h

 // 帧结构
struct FRAME
{
cv::Mat rgb, depth; //该帧对应的彩色图与深度图
cv::Mat desp; //特征描述子
vector<cv::KeyPoint> kp; //关键点
}; // PnP 结果
struct RESULT_OF_PNP
{
cv::Mat rvec, tvec;
int inliers;
}; // computeKeyPointsAndDesp 同时提取关键点与特征描述子
void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor ); // estimateMotion 计算两个帧之间的运动
// 输入:帧1和帧2, 相机内参
RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera );

  我们把关键帧和PnP的结果都封成了结构体,以便将来别的程序调用。这两个函数的实现如下:

  src/slamBase.cpp

 // computeKeyPointsAndDesp 同时提取关键点与特征描述子
void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor )
{
cv::Ptr<cv::FeatureDetector> _detector;
cv::Ptr<cv::DescriptorExtractor> _descriptor; cv::initModule_nonfree();
_detector = cv::FeatureDetector::create( detector.c_str() );
_descriptor = cv::DescriptorExtractor::create( descriptor.c_str() ); if (!_detector || !_descriptor)
{
cerr<<"Unknown detector or discriptor type !"<<detector<<","<<descriptor<<endl;
return;
} _detector->detect( frame.rgb, frame.kp );
_descriptor->compute( frame.rgb, frame.kp, frame.desp ); return;
} // estimateMotion 计算两个帧之间的运动
// 输入:帧1和帧2
// 输出:rvec 和 tvec
RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera )
{
static ParameterReader pd;
vector< cv::DMatch > matches;
cv::FlannBasedMatcher matcher;
matcher.match( frame1.desp, frame2.desp, matches ); cout<<"find total "<<matches.size()<<" matches."<<endl;
vector< cv::DMatch > goodMatches;
double minDis = ;
double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() );
for ( size_t i=; i<matches.size(); i++ )
{
if ( matches[i].distance < minDis )
minDis = matches[i].distance;
} for ( size_t i=; i<matches.size(); i++ )
{
if (matches[i].distance < good_match_threshold*minDis)
goodMatches.push_back( matches[i] );
} cout<<"good matches: "<<goodMatches.size()<<endl;
// 第一个帧的三维点
vector<cv::Point3f> pts_obj;
// 第二个帧的图像点
vector< cv::Point2f > pts_img; // 相机内参
for (size_t i=; 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 == )
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[][] = {
{camera.fx, , camera.cx},
{, camera.fy, camera.cy},
{, , }
}; cout<<"solving pnp"<<endl;
// 构建相机矩阵
cv::Mat cameraMatrix( , , CV_64F, camera_matrix_data );
cv::Mat rvec, tvec, inliers;
// 求解pnp
cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, , 1.0, , inliers ); RESULT_OF_PNP result;
result.rvec = rvec;
result.tvec = tvec;
result.inliers = inliers.rows; return result;
}

  此外,我们还实现了一个简单的参数读取类。这个类读取一个参数的文本文件,能够以关键字的形式提供文本文件中的变量:

  include/slamBase.h

 // 参数读取类
class ParameterReader
{
public:
ParameterReader( string filename="./parameters.txt" )
{
ifstream fin( filename.c_str() );
if (!fin)
{
cerr<<"parameter file does not exist."<<endl;
return;
}
while(!fin.eof())
{
string str;
getline( fin, str );
if (str[] == '#')
{
// 以‘#’开头的是注释
continue;
} int pos = str.find("=");
if (pos == -)
continue;
string key = str.substr( , pos );
string value = str.substr( pos+, str.length() );
data[key] = value; if ( !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;
};

  它读的参数文件是长这个样子的:

# 这是一个参数文件
# 去你妹的yaml! 我再也不用yaml了!简简单单多好! # part 4 里定义的参数 detector=SIFT
descriptor=SIFT
good_match_threshold=4 # camera
camera.cx=325.5;
camera.cy=253.5;
camera.fx=518.0;
camera.fy=519.0;
camera.scale=1000.0;

  嗯,参数文件里,以“变量名=值”的形式定义变量。以井号开头的是注释啦!是不是很简单呢?

  小萝卜:师兄你为何对yaml有一股强烈的怨念?

  师兄:哎,不说了……总之实现简单的功能,就用简单的东西,特别是从教程上来说更应该如此啦。

  现在,如果我们想更改特征类型,就只需在parameters.txt文件里进行修改,不必编译源代码了。这对接下去的各种调试都会很有帮助。


拼接点云

  点云的拼接,实质上是对点云做变换的过程。这个变换往往是用变换矩阵(transform matrix)来描述的:$$T=\left[ \begin{array}{ll} R_{3 \times 3} & t_{3 \times 1} \\ O_{1 \times 3} & 1 \end{array} \right] \in R^{4 \times 4} $$ 该矩阵的左上部分是一个$3 \times 3$的旋转矩阵,它是一个正交阵。右上部分是$3 \times 1$的位移矢量。左下是$3 \times 1$的缩放矢量,在SLAM中通常取成0,因为环境里的东西不太可能突然变大变小(又没有缩小灯)。右下角是个1. 这样的一个阵可以对点或者其他东西进行齐次变换:$$ \left[ \begin{array}{l} y_1 \\ y_2 \\ y_3 \\ 1 \end{array} \right] = T \cdot \left[ \begin{array}{l} x_1 \\ x_2 \\ x_3 \\ 1 \end{array} \right]$$

  由于变换矩阵结合了旋转和缩放,是一种较为经济实用的表达方式。它在机器人和许多三维空间相关的科学中都有广泛的应用。PCL里提供了点云的变换函数,只要给定了变换矩阵,就能对移动整个点云:

pcl::transformPointCloud( input, output, T );

  小萝卜:所以我们现在就是要把OpenCV里的旋转向量、位移向量转换成这个矩阵喽?

  师兄:对!OpenCV认为旋转矩阵$R$,虽然有$3\times 3$那么大,自由变量却只有三个,不够节省空间。所以在OpenCV里使用了一个向量来表达旋转。向量的方向是旋转轴,大小则是转过的弧度.

  小萝卜:但是我们又把它变成了矩阵啊,这不就没有意义了吗!

  师兄:呃,这个,确实如此。不管如何,我们先用罗德里格斯变换(Rodrigues)将旋转向量转换为矩阵,然后“组装”成变换矩阵。代码如下:

  src/joinPointCloud.cpp

 /*************************************************************************
> File Name: src/jointPointCloud.cpp
> Author: Xiang gao
> Mail: gaoxiang12@mails.tsinghua.edu.cn
> Created Time: 2015年07月22日 星期三 20时46分08秒
************************************************************************/ #include<iostream>
using namespace std; #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> int main( int argc, char** argv )
{
//本节要拼合data中的两对图像
ParameterReader pd;
// 声明两个帧,FRAME结构请见include/slamBase.h
FRAME frame1, frame2; //读取图像
frame1.rgb = cv::imread( "./data/rgb1.png" );
frame1.depth = cv::imread( "./data/depth1.png", -);
frame2.rgb = cv::imread( "./data/rgb2.png" );
frame2.depth = cv::imread( "./data/depth2.png", - ); // 提取特征并计算描述子
cout<<"extracting features"<<endl;
string detecter = pd.getData( "detector" );
string descriptor = pd.getData( "descriptor" ); computeKeyPointsAndDesp( frame1, detecter, descriptor );
computeKeyPointsAndDesp( frame2, detecter, descriptor ); // 相机内参
CAMERA_INTRINSIC_PARAMETERS camera;
camera.fx = atof( pd.getData( "camera.fx" ).c_str());
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() ); cout<<"solving pnp"<<endl;
// 求解pnp
RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera ); cout<<result.rvec<<endl<<result.tvec<<endl; // 处理result
// 将旋转向量转化为旋转矩阵
cv::Mat R;
cv::Rodrigues( result.rvec, R );
Eigen::Matrix3d r;
cv::cv2eigen(R, r); // 将平移向量和旋转矩阵转换成变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); Eigen::AngleAxisd angle(r);
cout<<"translation"<<endl;
Eigen::Translation<double,> trans(result.tvec.at<double>(,), result.tvec.at<double>(,), result.tvec.at<double>(,));
T = angle;
T(,) = result.tvec.at<double>(,);
T(,) = result.tvec.at<double>(,);
T(,) = result.tvec.at<double>(,); // 转换点云
cout<<"converting image to clouds"<<endl;
PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera ); // 合并点云
cout<<"combining clouds"<<endl;
PointCloud::Ptr output (new PointCloud());
pcl::transformPointCloud( *cloud1, *output, T.matrix() );
*output += *cloud2;
pcl::io::savePCDFile("data/result.pcd", *output);
cout<<"Final result saved."<<endl; pcl::visualization::CloudViewer viewer( "viewer" );
viewer.showCloud( output );
while( !viewer.wasStopped() )
{ }
return ;
}

  重点在于59至73行,讲述了这个转换的过程。

  变换完毕后,我们就得到了拼合的点云啦:

  怎么样?是不是有点成就感了呢?


接下来的事……

  至此,我们已经实现了一个只有两帧的SLAM程序。然而,也许你还不知道,这已经是一个视觉里程计(Visual Odometry)啦!只要不断地把进来的数据与上一帧对比,就可以得到完整的运动轨迹以及地图了呢!

  小萝卜:这听着已经像是SLAM了呀!

  师兄:嗯,要做完整的SLAM,还需要一些东西。以两两匹配为基础的里程计有明显的累积误差,我们需要通过回环检测来消除它。这也是我们后面几讲的主要内容啦!

  小萝卜:那下一讲我们要做点什么呢?

  师兄:我们先讲讲关键帧的处理,因为把每个图像都放进地图,会导致地图规模增长地太快,所以需要关键帧技术。然后呢,我们要做一个SLAM后端,就要用到g2o啦!


课后作业

  由于参数文件可以很方便地调节,请你试试不同的特征点类型,看看哪种类型比较符合你的心意。为此,最好在源代码中加入显示匹配图的代码哦!

未完待续


  如果你觉得我的博客有帮助,可以进行几块钱的小额赞助,帮助我把博客写得更好。

  

一起做RGB-D SLAM (4)的更多相关文章

  1. (2)RGB-D SLAM系列- 工具篇(依赖库及编译)

    做了个SLAM的小视频,有兴趣的朋友可以看下 https://youtu.be/z5wDzMZF10Q 1)Library depended 一个完整的SLAM系统包括,数据流获取,数据读取,特征提取 ...

  2. Android 音视频编解码——RGB与YUV格式转换

    一.RGB模型与YUV模型 1.RGB模型 我们知道物理三基色分别是红(Red).绿(Green).蓝(Blue).现代的显示器技术就是通过组合不同强度的红绿蓝三原色,来达成几乎任何一种可见光的颜色. ...

  3. 音视频编解码——RGB与YUV格式转换

    一.RGB模型与YUV模型 1.RGB模型 我们知道物理三基色分别是红(Red).绿(Green).蓝(Blue).现代的显示器技术就是通过组合不同强度的红绿蓝三原色,来达成几乎任何一种可见光的颜色. ...

  4. 常用的SLAM解决方案

    ORB SLAM 可以去Github上自己搜索现成的SLAM程序包 在此基础上做优化 视觉SLAM的分类方法:按摄像头的多少分为单目和双目,按是否使用概率方法分为概率法和图法 链接 学习SLAM重要的 ...

  5. 转:SLAM算法解析:抓住视觉SLAM难点,了解技术发展大趋势

    SLAM(Simultaneous Localization and Mapping)是业界公认视觉领域空间定位技术的前沿方向,中文译名为“同步定位与地图构建”,它主要用于解决机器人在未知环境运动时的 ...

  6. BAD SLAM:捆绑束调整直接RGB-D SLAM

    BAD SLAM:捆绑束调整直接RGB-D SLAM BAD SLAM: Bundle Adjusted Direct RGB-D SLAM 论文地址: http://openaccess.thecv ...

  7. Camera 图像处理原理分析

    1         前言 做为拍照手机的核心模块之一,camera sensor效果的调整,涉及到众多的参数,如果对基本的光学原理及sensor软/硬件对图像处理的原理能有深入的理解和把握的话,对我们 ...

  8. Camera图像处理原理及实例分析-重要图像概念

    Camera图像处理原理及实例分析 作者:刘旭晖  colorant@163.com  转载请注明出处 BLOG:http://blog.csdn.net/colorant/ 主页:http://rg ...

  9. Android Camera2采集摄像头原始数据并手动预览

    Android Camera2采集摄像头原始数据并手动预览 最近研究了一下android摄像头开发相关的技术,也看了Google提供的Camera2Basic调用示例,以及网上一部分代码,但都是在Te ...

随机推荐

  1. java代码-----实现有键盘获得的字符串存储在文件中,并从文件中读取后显示在屏幕上

    总结: 没体会到 package com.a.b; import java.io.*; public class tsetOut { public static void main(String[] ...

  2. 【转】WebAPI使用多个xml文件生成帮助文档

    来自:http://www.it165.net/pro/html/201505/42504.html 一.前言 上篇有提到在WebAPI项目内,通过在Nuget里安装(Microsoft.AspNet ...

  3. USB驱动程序之USB设备驱动程序1简单编写

    1.驱动编写分析 (1)usb总线驱动程序在我们接入USB设备的时候会帮我们构造一个新的usb_device.注册到总线里面来.左边这一块已经帮我们做好了,我们要做的是右边这一块.我们要构造一个usb ...

  4. 大白话系列之C#委托与事件讲解大结局

    声明:本系列非原创,因为太精彩才转载,如有侵权请通知删除,原文:http://www.cnblogs.com/wudiwushen/archive/2010/04/20/1698795.html 今天 ...

  5. Druid.io系列(六):问题总结

    原文地址: https://blog.csdn.net/njpjsoftdev/article/details/52956508 我们在生产环境中使用Druid也遇到了很多问题,通过阅读官网文档.源码 ...

  6. sql server xml 截断

    c#读取 sql生成的xml时,发生阶段. 加,type 解决

  7. 3、数据类型一:strings

    题外: 学习过程参考三份资料:<Redis入门指南>.<Redis实战>.http://redis.io 后面的学习笔记中会引入它们的内容或代码,在这里统一说明,后面笔记中便不 ...

  8. HTTP接口开发专题四(接收http接口发送过来的请求)

    前面讲了调用http接口的操作,这篇讲下接收http接口的操作.(以Spring MVC为例) 1)如果发送过来的内容类型是application/x-www-form-urlencoded ,则按照 ...

  9. C#如何消除绘制图形缩放时抖动,总结

    一.手动双缓冲 首先定义一个BitmapBitmap backBuffer = new Bitmap(画布宽度, 画布高度);然后获取这个Bitmap的GraphicsGraphics graphic ...

  10. 跟着太白老师学python day11 闭包 及在爬虫中的基本使用

    闭包的基本概念: 闭包 内层函数对外层函数的变量(不包括全局变量)的引用,并返回,这样就形成了闭包 闭包的作用:当程序执行时,遇到了函数执行,它会在内存中开辟一个空间,如果这个函数内部形成了闭包, 那 ...