slam1-粮草储备

没有介绍

这篇开始陆陆续续写一些关于slam的东西。为什么要写slam?

  • 波士顿动力机器人都那么牛逼了,我们不学点,真的对不起我的这个专业(控制科班出身)
  • 我非常喜欢机器人,甚至想自己做一个机器人
  • 搞点实业

好了,那闲话不多说。

重要的网站

  1. https://openslam-org.github.io OpenSlam的官网,这里面很多slam相关的项目,必须收藏;
  2. https://pdfs.semanticscholar.org/1c7e/7af4388b17b137badc6ec19e9724e6bd91e4.pdf 这个文档讲解一个200行代码的slam的设计
  3. http://web2py.iiit.ac.in/research_centres/publications/download/mastersthesis.pdf.8ba0db3a8804a43d.52616a5468657369732e706466.pdf 这篇文档讲解了一个完整的slam算法应该如何设计。
  4. https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-412j-cognitive-robotics-spring-2005/projects/1aslam_blas_repo.pdf 给新手看的slam文档。
  5. https://people.csail.mit.edu/sparis/talks/Paris_06_3D_Reconstruction.pdf 这篇文章讲的就是3d重建。
  6. http://www.cvlibs.net/software/libviso/ 这很重要了,划重点,这个是视觉里程计的最经典库。
  7. http://ccwu.me/vsfm/ 这是一个常见的3维重建的软件,比较重要.

做一些小探索

抛出一篇经典的3D重建的论文,这篇论文引用非常多。地址为: http://people.reed.edu/~jimfix/math385/lec01.1/Capture/Photo_Tourism.pdf

地图构建

简单来说,地图构建有两种方式,一种是事先人工采集,一种是让机器人自动的去建图。以下两种方法是用于地图构建的:

  • A: Dijkstra算法的扩展, 听起来就很复杂, *这个算法很有意思,大家可以实现以下;
  • Converge problem;

在进行slam的过程中,先不要考虑导航,先解决地图构建的问题。那就得思考一下这么一个问题:

  • 机器人如何构建地图?
  • 地图是什么?什么样的地图?什么形式?

这都是需要解决的根本问题。

单目稠密地图构建

说太多,都不如直接看一个代码的运行效果,先记录一下:

======================= 引用

在上述中,我们讨论的是稀疏地图的构建,但是在实际的定位、导航和壁障过程中,我们需要有稠密地图。常见的单目稠密地图的构建思路有:
1.单目:通过运动,得出运动轨迹,计算出运动的关系,通过三角测量计算出像素深度。(同下)
2.双目:利用俩个相机的视差计算出像素的深度。(吃力不讨好,但是大型场合可能有比较好的效果。)
3.RGBD:自带深度图,可直接得到像素的深度。(比较好,但是不适合大型场合)

对于单目而言,即我们提到的第一种方法,通过单目建立稠密地图:
1.提取特征点,然后在很多图像中,进行特征点的匹配,跟踪特征点在各张图像的轨迹(经常用ORB特征提取)
2.然后通过三角测量估计每一个像素的深度。在这里,我们需要利用很多的三角测量使得某点的深度进行收敛,得到确切的深度信息。

在稠密深度地图中,无法对每个像素计算其描述子,所以在稠密估计问题中,匹配成为很重要的一环,我们用到了极线搜索和块匹配技术,极线搜索的原理如下图所示:

而块匹配技术就相当于把像素的比较换成了块的比较,我们直接上代码可得:

#include <iostream>
#include <vector>
#include <fstream>
using namespace std;
#include <boost/timer.hpp>
// for sophus
#include <sophus/se3.h> //这里使用了sophus这个工具,使用SE3
using Sophus::SE3;
// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;
// ------------------------------------------------------------------
// parameters
const int boarder = 20; // 边缘宽度
const int width = 640; // 宽度
const int height = 480; // 高度
const double fx = 481.2f; // 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 2; // NCC 取的窗口半宽度
const int ncc_area = (2*ncc_window_size+1)*(2*ncc_window_size+1); // NCC窗口面积
const double min_cov = 0.1; // 收敛判定:最小方差
const double max_cov = 10; // 发散判定:最大方差
// ------------------------------------------------------------------
// 重要的函数
// 从 REMODE 数据集读取数据
bool readDatasetFiles(
const string& path,
vector<string>& color_image_files,
vector<SE3>& poses
);
// 根据新的图像更新深度估计
bool update(
const Mat& ref,
const Mat& curr,
const SE3& T_C_R,
Mat& depth,
Mat& depth_cov
);
// 极线搜索
bool epipolarSearch(
const Mat& ref,
const Mat& curr,
const SE3& T_C_R,
const Vector2d& pt_ref,
const double& depth_mu,
const double& depth_cov,
Vector2d& pt_curr
);
// 更新深度滤波器
bool updateDepthFilter(
const Vector2d& pt_ref,
const Vector2d& pt_curr,
const SE3& T_C_R,
Mat& depth,
Mat& depth_cov
);
// 计算 NCC 评分
double NCC( const Mat& ref, const Mat& curr, const Vector2d& pt_ref, const Vector2d& pt_curr );
// 双线性灰度插值
inline double getBilinearInterpolatedValue( const Mat& img, const Vector2d& pt ) {
uchar* d = & img.data[ int(pt(1,0))*img.step+int(pt(0,0)) ];
double xx = pt(0,0) - floor(pt(0,0));
double yy = pt(1,0) - floor(pt(1,0));
return (( 1-xx ) * ( 1-yy ) * double(d[0]) +
xx* ( 1-yy ) * double(d[1]) +
( 1-xx ) *yy* double(d[img.step]) +
xx*yy*double(d[img.step+1]))/255.0;
}
// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
bool plotDepth( const Mat& depth );
// 像素到相机坐标系
inline Vector3d px2cam ( const Vector2d px ) {
return Vector3d (
(px(0,0) - cx)/fx,
(px(1,0) - cy)/fy,
1
);
}
// 相机坐标系到像素
inline Vector2d cam2px ( const Vector3d p_cam ) {
return Vector2d (
p_cam(0,0)*fx/p_cam(2,0) + cx,
p_cam(1,0)*fy/p_cam(2,0) + cy
);
}
// 检测一个点是否在图像边框内
inline bool inside( const Vector2d& pt ) {
return pt(0,0) >= boarder && pt(1,0)>=boarder
&& pt(0,0)+boarder<width && pt(1,0)+boarder<=height;
}
// 显示极线匹配
void showEpipolarMatch( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_curr );
// 显示极线
void showEpipolarLine( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_min_curr, const Vector2d& px_max_curr );
// ------------------------------------------------------------------
int main( int argc, char** argv )
{
// 从数据集读取数据
vector<string> color_image_files;
vector<SE3> poses_TWC;
//该函数的功能是将图片名和位姿从文件中提取出来并进行存储
//其中“/home/.../data”是存放文件的路径
bool ret = readDatasetFiles(“/home/.../data”, color_image_files, poses_TWC );
if ( ret==false )
{
cout<<"Reading image files failed!"<<endl;
return -1;
}
cout<<"read total "<<color_image_files.size()<<" files."<<endl;
// 第一张图
Mat ref = imread( color_image_files[0], 0 ); // gray-scale image
SE3 pose_ref_TWC = poses_TWC[0];
double init_depth = 3.0; // 深度初始值
double init_cov2 = 3.0; // 方差初始值
Mat depth( height, width, CV_64F, init_depth ); // 深度图
Mat depth_cov( height, width, CV_64F, init_cov2 ); // 深度图方差
for ( int index=1; index<color_image_files.size(); index++ )
{
cout<<"*** loop "<<index<<" ***"<<endl;
Mat curr = imread( color_image_files[index], 0 );
if (curr.data == nullptr) continue;
SE3 pose_curr_TWC = poses_TWC[index];
SE3 pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; // 坐标转换关系: T_C_W * T_W_R = T_C_R
update( ref, curr, pose_T_C_R, depth, depth_cov );
plotDepth( depth );
imshow("image", curr);
waitKey(1);
}
cout<<"estimation returns, saving depth map ..."<<endl;
imwrite( "depth.png", depth );
cout<<"done."<<endl;
return 0;
}
bool readDatasetFiles(
const string& path,
vector< string >& color_image_files,
std::vector<SE3>& poses
)
{
ifstream fin( path+"/first_200_frames_traj_over_table_input_sequence.txt");
if ( !fin ) return false;
while ( !fin.eof() )
{
// 数据格式:图像文件名 tx, ty, tz, qx, qy, qz, qw ,注意是 TWC 而非 TCW
string image;
fin>>image;
double data[7];
for ( double& d:data ) fin>>d;
color_image_files.push_back( path+string("/images/")+image );
poses.push_back(
SE3( Quaterniond(data[6], data[3], data[4], data[5]),
Vector3d(data[0], data[1], data[2]))
);
if ( !fin.good() ) break;
}
return true;
}
// 对整个深度图进行更新
bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat& depth_cov )
{
#pragma omp parallel for
for ( int x=boarder; x<width-boarder; x++ )
#pragma omp parallel for
for ( int y=boarder; y<height-boarder; y++ )
{
// 遍历每个像素
if ( depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov ) // 深度已收敛或发散
continue;
// 在极线上搜索 (x,y) 的匹配
Vector2d pt_curr;
bool ret = epipolarSearch (
ref,
curr,
T_C_R,
Vector2d(x,y),
depth.ptr<double>(y)[x],
sqrt(depth_cov.ptr<double>(y)[x]),
pt_curr
);
if ( ret == false ) // 匹配失败
continue;
// 取消该注释以显示匹配
// showEpipolarMatch( ref, curr, Vector2d(x,y), pt_curr );
// 匹配成功,更新深度图
updateDepthFilter( Vector2d(x,y), pt_curr, T_C_R, depth, depth_cov );
}
}
// 极线搜索
bool epipolarSearch(
const Mat& ref, const Mat& curr,
const SE3& T_C_R, const Vector2d& pt_ref,
const double& depth_mu, const double& depth_cov,
Vector2d& pt_curr )
{
Vector3d f_ref = px2cam( pt_ref );
f_ref.normalize();
Vector3d P_ref = f_ref*depth_mu; // 参考帧的 P 向量
Vector2d px_mean_curr = cam2px( T_C_R*P_ref ); // 按深度均值投影的像素
double d_min = depth_mu-3*depth_cov, d_max = depth_mu+3*depth_cov;
if ( d_min<0.1 ) d_min = 0.1;
Vector2d px_min_curr = cam2px( T_C_R*(f_ref*d_min) ); // 按最小深度投影的像素
Vector2d px_max_curr = cam2px( T_C_R*(f_ref*d_max) ); // 按最大深度投影的像素
Vector2d epipolar_line = px_max_curr - px_min_curr; // 极线(线段形式)
Vector2d epipolar_direction = epipolar_line; // 极线方向
epipolar_direction.normalize();
double half_length = 0.5*epipolar_line.norm(); // 极线线段的半长度
if ( half_length>100 ) half_length = 100; // 我们不希望搜索太多东西
// 取消此句注释以显示极线(线段)
// showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );
// 在极线上搜索,以深度均值点为中心,左右各取半长度
double best_ncc = -1.0;
Vector2d best_px_curr;
for ( double l=-half_length; l<=half_length; l+=0.7 ) // l+=sqrt(2)
{
Vector2d px_curr = px_mean_curr + l*epipolar_direction; // 待匹配点
if ( !inside(px_curr) )
continue;
// 计算待匹配点与参考帧的 NCC
double ncc = NCC( ref, curr, pt_ref, px_curr );
if ( ncc>best_ncc )
{
best_ncc = ncc;
best_px_curr = px_curr;
}
}
if ( best_ncc < 0.85f ) // 只相信 NCC 很高的匹配
return false;
pt_curr = best_px_curr;
return true;
}
double NCC (
const Mat& ref, const Mat& curr,
const Vector2d& pt_ref, const Vector2d& pt_curr
)
{
// 零均值-归一化互相关
// 先算均值
double mean_ref = 0, mean_curr = 0;
vector<double> values_ref, values_curr; // 参考帧和当前帧的均值
for ( int x=-ncc_window_size; x<=ncc_window_size; x++ )
for ( int y=-ncc_window_size; y<=ncc_window_size; y++ )
{
double value_ref = double(ref.ptr<uchar>( int(y+pt_ref(1,0)) )[ int(x+pt_ref(0,0)) ])/255.0;
mean_ref += value_ref;
double value_curr = getBilinearInterpolatedValue( curr, pt_curr+Vector2d(x,y) );
mean_curr += value_curr;
values_ref.push_back(value_ref);
values_curr.push_back(value_curr);
}
mean_ref /= ncc_area;
mean_curr /= ncc_area;
// 计算 Zero mean NCC
double numerator = 0, demoniator1 = 0, demoniator2 = 0;
for ( int i=0; i<values_ref.size(); i++ )
{
double n = (values_ref[i]-mean_ref) * (values_curr[i]-mean_curr);
numerator += n;
demoniator1 += (values_ref[i]-mean_ref)*(values_ref[i]-mean_ref);
demoniator2 += (values_curr[i]-mean_curr)*(values_curr[i]-mean_curr);
}
return numerator / sqrt( demoniator1*demoniator2+1e-10 ); // 防止分母出现零
}
bool updateDepthFilter(
const Vector2d& pt_ref,
const Vector2d& pt_curr,
const SE3& T_C_R,
Mat& depth,
Mat& depth_cov
)
{
// 我是一只喵
// 不知道这段还有没有人看
// 用三角化计算深度
SE3 T_R_C = T_C_R.inverse();
Vector3d f_ref = px2cam( pt_ref );
f_ref.normalize();
Vector3d f_curr = px2cam( pt_curr );
f_curr.normalize();
// 方程
// d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
// => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t]
// [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t]
// 二阶方程用克莱默法则求解并解之
Vector3d t = T_R_C.translation();
Vector3d f2 = T_R_C.rotation_matrix() * f_curr;
Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );
double A[4];
A[0] = f_ref.dot ( f_ref );
A[2] = f_ref.dot ( f2 );
A[1] = -A[2];
A[3] = - f2.dot ( f2 );
double d = A[0]*A[3]-A[1]*A[2];
Vector2d lambdavec =
Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
-A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
Vector3d xm = lambdavec ( 0,0 ) * f_ref;
Vector3d xn = t + lambdavec ( 1,0 ) * f2;
Vector3d d_esti = ( xm+xn ) / 2.0; // 三角化算得的深度向量
double depth_estimation = d_esti.norm(); // 深度值
// 计算不确定性(以一个像素为误差)
Vector3d p = f_ref*depth_estimation;
Vector3d a = p - t;
double t_norm = t.norm();
double a_norm = a.norm();
double alpha = acos( f_ref.dot(t)/t_norm );
double beta = acos( -a.dot(t)/(a_norm*t_norm));
double beta_prime = beta + atan(1/fx);
double gamma = M_PI - alpha - beta_prime;
double p_prime = t_norm * sin(beta_prime) / sin(gamma);
double d_cov = p_prime - depth_estimation;
double d_cov2 = d_cov*d_cov;
// 高斯融合
double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2);
double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 );
depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse;
depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2;
return true;
}
bool plotDepth(const Mat& depth)
{
imshow( "depth", depth*0.4 );
waitKey(1);
}

CMakeLists 文件为:

cmake_minimum_required(VERSION 2.8.3)
project(dense_mapping)
set(CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3 -fopenmp")
set(OpenCV_DIR /usr/local/opencv320/share/OpenCV)
#set(Sophus_LIBRARIES libSophus.so)
find_package(OpenCV 3.2 REQUIRED )
include_directories(SYSTEM ${OpenCV_INCLUDE_DIRS} /usr/local/opencv320/include)
find_package( "/usr/local/opencv320/include/opencv2" )
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
set( Sophus_INCLUDE_DIRS "/usr/local/include" )
set( Sophus_LIBS "/usr/local/lib/libSophus.so" )
include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${Sophus_LIBS}
)
add_executable(dense_mapping src/dense_mapping.cpp)
target_link_libraries(dense_mapping
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${EIGEN3_INCLUDE_DIR}
${Sophus_LIBS}
)