南柯一梦
0%

cartographer阅读笔记

发表于 更新于 分类于
cartographer 源码阅读中的一些记录

一些不熟悉的C++知识

std::move()

std::move 右值引用

左值是指表达式结束后依然存在的持久化对象，右值是指表达式结束时就不再存在的临时对象

在C++11之前，右值是不能引的，如

1
2
int &a = 1;			// error: 非常量的引用必须为左值
const int &a = 1;	// 我们最多只能用常量引用来绑定一个右值

在C++11中我们可以引用右值，使用&&实现：

1
int &&a = 1;

右值引用的目的

  　1. 消除两个对象交互时不必要的对象拷贝，节省运算存储资源，提高效率。
  　2. 能够更简洁明确地定义泛型函数。

例子

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#include <iostream>
#include <utility>
#include <vector>
#include <string>
int main()
{
    std::string str = "Hello";
    std::vector<std::string> v;
    //调用常规的拷贝构造函数，新建字符数组，拷贝数据
    v.push_back(str);
    std::cout << "After copy, str is \"" << str << "\"\n";
    //调用移动构造函数，掏空str，掏空后，最好不要使用str
    v.push_back(std::move(str));
    std::cout << "After move, str is \"" << str << "\"\n";
    std::cout << "The contents of the vector are \"" << v[0] << "\", \"" << v[1] << "\"\n";
}

输出

1
2
3
After copy, str is "Hello"
After move, str is ""
The contents of the vector are "Hello", "Hello"

std::unique_ptr

无法复制，只能移动的智能指针

函数重载和重写虚函数

在C++中同名函数有三种关系：

  • 重载 (overlode)：相同作用域；函数名相同；参数列表不同；返回类型随意。
  • 覆盖 (override)：不同作用域下（分别在父类和子类中）；函数名相同；参数列表列表相同；返回类型相同（协变除外）；基类函数必须有virtual修饰；父类和子类的访问限定可以不同。
  • 隐藏 (overhide)：不同作用域下（分别在父类和子类中）；函数名相同；除过覆盖的同名函数都是隐藏关系。

强制类型转换

c++除了能使用c语言的强制类型转换外，还新增了四种强制类型转换：static_cast、dynamic_cast、const_cast、reinterpret_cast，主要运用于继承关系类间的强制转化，语法为：

1
2
3
4
static_cast<new_type>      (expression)
dynamic_cast<new_type>     (expression) 
const_cast<new_type>       (expression) 
reinterpret_cast<new_type> (expression)

备注：new_type为目标数据类型，expression为原始数据类型变量或者表达式。

1
2
3
4
5
6
7
8
9
10
11
char a = 'a';
int b = static_cast<char>(a);//正确，将char型数据转换成int型数据

double *c = new double;
void *d = static_cast<void*>(c);//正确，将double指针转换成void指针

int e = 10;
const int f = static_cast<const int>(e);//正确，将int型数据转换成const int型数据

const int g = 20;
int *h = static_cast<int*>(&g);//编译错误，static_cast不能转换掉g的const属性

using

除了命名变量空间以外，using还可以作为类型重命名，例如这两个写法是等价的：

1
2
typedef std::vector<int> intvec;
using intvec = std::vector<int>;

智能for

格式

1
2
for(auto iter：itered){    
}

global_trajectory_builder.cc

void AddSensorData()

1
void AddSensorData(const std::string& sensor_id,const sensor::TimedPointCloudData& timed_point_cloud_data) override

在这个GlobalTrajectoryBuilder类的AddSensorData()函数里面首先调用类的私有成员变量local_trajectory_builder_的AddRangeData()成员函数，注意这个AddRangeData已经不是GlobalTrajectoryBuilder类中的AddSensorData()函数了。

local_trajectory_builder_2d.cc

本文件重点 帧间匹配

filtered_gravity_aligned_point_cloud

点云数据的重力矫正

室内场景中地板是水平的， 墙壁是竖直的， 所以点云数据的重力方向应该尽可能与场景中的法线方向垂直或平行。而人工拍摄得到的深度图像因为各种因素影响很可能重力方向是倾斜的， 所以本文对点云进行重力矫正， 将其进行旋转使之与场景坐标系对准。

ScanMatch()

real_time_correlative_scan_matcher_得到一个位置的解，用这个作为初始解用ceres_scan_matcher_优化的方法优化解。

correlative_scan_matcher_的方法是基于搜索的方法。

ceres_scan_matcher_的方法是基于优化的方法，优化的过程中采用梯度下降的方法，采用三次样条差值的方法使得离散栅格连续化。这种局部优化的方法很容易陷入到局部极小值当中，因此这个方法能正常工作的前提是初始值离全局最优值比较近。

DiscreteScan2D

1
2
3
typedef std::vector<Eigen::Array2i> DiscreteScan2D 

typedef Eigen::Array<int, 2, 1> Eigen::Array2i

TSDF

参考：https://www.jianshu.com/p/462fe75753f7

ceres库中的一些术语

优化目标 \[ \begin{split}\min_{\mathbf{x}} &\quad \frac{1}{2}\sum_{i} \rho_i\left(\left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2\right) \\ \text{s.t.} &\quad l_j \le x_j \le u_j\end{split} \] \(\rho()\) :loss function

\(f(x)\) :cost function

求和符号中的每一项构成一个residual block

map

cartographer中的点云表达

cartographer中的点是自己封装对Eigen库的3*1的matrix做了一层结构体封装，原型是

1
2
3
4
5
6
7
8
9
//不带时间信息的
struct RangefinderPoint {
  Eigen::Vector3f position;
};
//带时间信息的
struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time;
};

使用的时候进行了重命名

1
using PointType = RangefinderPoint;

点云类PointCloud的核心就是其私有的成员变量，points_，如下：

1
2
3
4
class PointCloud {
 private:
	  std::vector<PointType> points_;
};

PointCloud，此外还封装了一些诸如添加点，大小等函数。

一帧激光的点云数据就是在此基础上再进行一次封装。

1
2
3
4
5
struct RangeData {
  Eigen::Vector3f origin; //原点
  PointCloud returns;     //返回的
  PointCloud misses;      //miss点 无反射的
};

注意这个结构体里面的misses点指得是发射出的激光里面那些没有收到反射值（测距超过量程了）的点，不是一般所指的那些激光线束所经过而代表的空闲点。

cartographer中的地图的一些表达

地图存的是一维数据，查询时先将二维xy转换为一维索引，然后查询。

重要基类 Grid2D，私有成员变量情况如下

1
2
3
4
5
6
7
8
9
10
11
12
class Grid2D : public GridInterface {
 private:
  MapLimits limits_;
  std::vector<uint16> correspondence_cost_cells_; //记录各个栅格单元空闲概率的列表
  float min_correspondence_cost_;                 //Pfree的最小值
  float max_correspondence_cost_;                 //Pfree的最大值
  std::vector<int> update_indices_;               //记录跟新过的栅格单元的索引

  // Bounding box of known cells to efficiently compute cropping limits.
  Eigen::AlignedBox2i known_cells_box_;           //一个用于记录哪个栅格单元有值的数据结构
  const std::vector<float>* value_to_correspondence_cost_table_;
};

概率栅格地图继承自Grid2D

1
2
3
4
class ProbabilityGrid : public Grid2D {
    private:
     ValueConversionTables* conversion_tables_;
};

其中重要的函数，初始化栅格地图的函数如下

1
2
3
4
5
6
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,const float probability) {
  uint16& cell = (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
  CHECK_EQ(cell, kUnknownProbabilityValue);
  cell =  CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
  mutable_known_cells_box()->extend(cell_index.matrix());
}

一组概率

cartographer中一般叫Probability的都是占有概率，叫CorrespondenceCost的都是空闲概率。

关于向地图中插入一组新的激光数据

核心函数

1
2
3
4
void CastRays(const sensor::RangeData& range_data, 	//待插入的扫描数据
              const std::vector<uint16>& hit_table,	//预先计算的hit表
              const std::vector<uint16>& miss_table,//预先计算的miss表
              const bool insert_free_space, ProbabilityGrid* probability_grid)

hit_table和miss_table

提前计算好的一个occupy栅格再次被击中后应当更新的栅格值，即：

\[ hit\_table\_[Grid_{value\_old}] = Grid_{value\_new} \] miss_table同理。

核心步骤

  1. 记录击中点在栅格中的位置（代码中用end来记录，意思应该是指代表极光线束的末端），更新地图

    1
    2
    3
    4
    for (const sensor::RangefinderPoint& hit : range_data.returns) {
     ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
        probability_grid->ApplyLookupTable(ends.back()/kSubpixelScale, hit_table);
     }

  2. 记录原点到每一个end点之间的miss点，然后用这些点更新地图

    1
    2
    3
    4
    5
    6
    for (const Eigen::Array2i& end : ends) {
      std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, kSubpixelScale);
      for (const Eigen::Array2i& cell_index : ray) {
        probability_grid->ApplyLookupTable(cell_index, miss_table);
      }
    }

  3. 上面两步骤中的跟新地图都通过调运probability_grid->ApplyLookupTable()函数完成。函数中最关键的一步即为

    1
    *cell = table[*cell];