障碍约束和雅格比矩阵的推导|路径规划TEB算法各类约束及雅格比矩阵

障碍约束定义的误差函数表示机器人到障碍的最小距离。 $ \min \textrm{penaltyBelow}( dist2point ) \cdot weight $. 

1
2
3
4
5
6
7
8
9
10
11
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
// calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) 计算机器人到障碍物的距离
// current_pose为当前机器人的位姿。
double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);

if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
  _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, 
  										cfg_->optim.obstacle_cost_exponent);
}

obstacle_cost_exponent和min_obstacle_dist的设置，一般都会进入if情况。 注意 max cost (before weighting) is the same as the straight line version and that all other costs are below the straight line (for positive exponent), so it may be necessary to increase weight_obstacle and/or the inflation_weight when using larger exponents.

时间约束和kinematics约束及雅格比矩阵的推导|路径规划TEB算法各类约束及雅格比矩阵

时间约束

EdgeTimeOptimal类太简单了，误差函数 _error[0] = timediff->dt();，优化 ，那么对其求偏导，显然只有一个矩阵，一个元素 1 ： _jacobianOplusXi( 0 , 0 ) = 1;

kinematics 约束

两个误差方程，一个是 non-holonomic约束， ：

初始配置和方向之间的角度 必须等于配置和方向之间的夹角，即

根据二维叉积 A×B=|A| |B|⋅sinα 得到

所以代码中的目标函数写成

另一个是 positive-drive-direction约束

两个configure，所以两个雅格比，维度明显是 2x3，两个误差方程分别对(x, y, angle)求偏导，源码里的求导很简单，还不如速度约束的求导复杂，就不写过程了。

值得注意的是绝对值的求导结果会用sign函数表示。

加速度约束和雅格比矩阵的推导|路径规划TEB算法各类约束及雅格比矩阵

默认为5元边约束。 但是包含3个类：EdgeAcceleration, EdgeAccelerationStart, EdgeAccelerationGoal，后两个为3元边

向量的维度是2，第一个元素代表线加速度，第二个是角加速度。
EdgeAccelerationStart() and EdgeAccelerationGoal() 用于边界值

加速度约束和速度约束类似，不过变成两段圆弧和角度差、两个时间差、两个线速度和角速度。两个线速度的差除以两个时间差之和就是加速度。

1
2
3
4
5
6
7
8
9
double vel1 = dist1 / dt1->dt();
double vel2 = dist2 / dt2->dt();
const double acc_lin  = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon);

const double omega1 = angle_diff1 / dt1->dt();
const double omega2 = angle_diff2 / dt2->dt();
const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);

起点的加速度约束

注意：起点加速度约束和终点加速度约束都是3元边。

对于起点的加速度约束，源码有以下变化:

1
2
3
4
5
6
7
8
9
void setInitialVelocity(const geometry_msgs::Twist& vel_start)
{
	_measurement = &vel_start;
}

ROS_ASSERT_MSG(cfg_ && _measurement, 
"You must call setTebConfig() and setInitialVelocity() on EdgeAccelerationStart()");
vel1 = _measurement->linear.x;
omega1 = _measurement->angular.z;

其他全一样，对于终点的加速度约束，则是vel2和omega2换成了_measurement的成员。那么这个_measurement从何而来？

在TebOptimalPlanner::plan里有一段:

1
2
3
4
5
6
if (start_vel)
  setVelocityStart(*start_vel);
if (free_goal_vel)
  setVelocityGoalFree();
else
  vel_goal_.first = true;

setVelocityStart给起始速度赋值：

1
2
3
4
5
6
7
8
9
// std::pair<bool, geometry_msgs::Twist>  vel_start_;
// 注意vel_start_ 和 vel_start 不同
void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist& vel_start)
{
  vel_start_.first = true;
  vel_start_.second.linear.x = vel_start.linear.x;
  vel_start_.second.linear.y = vel_start.linear.y;
  vel_start_.second.angular.z = vel_start.angular.z;
}

然后到了AddEdgesAcceleration里添加起始加速度约束，才用到vel_start_

1
2
3
4
5
6
7
8
9
10
11
12
if (vel_start_.first)
{
  EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart;
  acceleration_edge->setVertex(0,teb_.PoseVertex(0));
  acceleration_edge->setVertex(1,teb_.PoseVertex(1));
  acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
  // 这里就是上面的 setInitialVelocity, _measurement 就是 vel_start_.second
  acceleration_edge->setInitialVelocity(vel_start_.second);
  acceleration_edge->setInformation(information);
  acceleration_edge->setTebConfig(*cfg_);
  optimizer_->addEdge(acceleration_edge);
}

AddEdgesAcceleration然后添加普通的加速度约束，再就是终点的加速度约束，这里就奇怪了，相应的setVelocityGoal函数没有调用的地方，这里是vel_goal_.second唯一赋值的地方。即使free_goal_vel为false，也没有赋值，意思是终点的速度为0？

雅格比矩阵的推导可以参照速度约束的，不同的地方是这次把角度单独拿出来了，这样有了8个雅格比

1
2
3
4
5
6
7
8
_jacobianOplus[0].resize(2,2); // conf1
_jacobianOplus[1].resize(2,2); // conf2
_jacobianOplus[2].resize(2,2); // conf3
_jacobianOplus[3].resize(2,1); // deltaT1
_jacobianOplus[4].resize(2,1); // deltaT2
_jacobianOplus[5].resize(2,1); // angle1
_jacobianOplus[6].resize(2,1); // angle2
_jacobianOplus[7].resize(2,1); // angle3

既然拿出了角度，那么对conf的雅格比矩阵的维度就变成了 2x2，不是2x3。两个误差函数，一个是线速度的，一个是角速度的

代码中的 double aux0 = 2/sum_time_inv; 错了 应当是 double aux0 = 2/sum_time;
J[0](0,0)的推导过程
其他项以此类推

g2o的详细使用|SLAM工具

G2O 与ceres风格不同，它是专门用于进行图优化的库，提供了大量预先定义的节点和边供用户调用。

曲线拟合的例子其实不太适合用来理解g2o，它涉及的顶点只有一个，边是一元边，有很多个。还是用图优化的例子更容易理解。

TEB算法的base_teb_edges.h, g2o_types\vertex_pose.h 和 g2o_types\vertex_timediff.h很有学习价值，以及edge_obstacle.h, edge_shortest_path.h, edge_velocity.h

顶点

最基础的BaseVertex类，template <int D, typename T> class BaseVertex，

  • D: minimal dimension of the vertex, 例如 3 for rotation in 3D. D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示
  • T: 节点估计值的类型,例如 Quaternion for rotation in 3D

g2o本身内部定义了一些常用的顶点类型

  • VertexSE2 : public BaseVertex<3, SE2> // 2D pose Vertex, (x,y,theta)
  • VertexSE3 : public BaseVertex<6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (注意不管四元数的 w)
  • VertexPointXY : public BaseVertex<2, Vector2>
  • VertexPointXYZ : public BaseVertex<3, Vector3>
  • VertexSBAPointXYZ : public BaseVertex<3, Vector3>
  • VertexSE3Expmap : public BaseVertex<6, SE3Quat>

如果我们需要的顶点类型这里面没有，就得自己定义了

重新定义顶点一般需要考虑重写如下函数：

1
2
3
4
5
# 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下，作为空函数
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

setToOriginImpl 和 _estimate

setToOriginImpl：顶点重置函数，设定被优化变量的原始值。有时可以不用实现。具体来说就是初始化成员变量 _estimate， _estimate的类型就是继承 BaseVertex时的模板参数

如果_estimate是Eigen::Vector3d，那么setToOriginImpl的内容可以是: _estimate << 0,0,0;。 如果是PoseSE2， _estimate的赋值可以这样：

1
2
3
4
5
6
7
8
9
_estimate.position() = position;
_estimate.x() = x;
_estimate.y() = y;
_estimate.theta() = theta;

virtual void setToOriginImpl()
{
    _estimate.setZero();
}

oplusImpl

顶点更新函数。非常重要的一个函数，主要用于优化过程中增量 △x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

比较简单的类型是这样实现：

1
2
3
4
5
6
7
8
9
10
11
// double 也可以换成 number_t
virtual void oplusImpl( const double* update ) // 更新
{
     // update为增量△m 迭代的时候，估计值 m= m + △m, m是向量（a,b,c）
    _estimate += Eigen::Vector3d(update);  
}

virtual void oplusImpl(const double* update)
{
    _estimate.plus(update);
}

李代数的类VertexSE3Expmap，上面两个函数是

1
2
3
4
5
6
7
8
9
virtual void setToOriginImpl() {    // 重置
    _estimate = SE3Quat();
}

virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    //更新方式 类似于左扰动？
    setEstimate(SE3Quat::exp(update)*estimate());        
}

边

g2o自带的边

  • EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>
  • EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>
  • EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>
仅优化一个变量的时候选择单边

以一元边为例template <int D, typename E, typename VertexXi> class BaseUnaryEdge : public BaseEdge<D,E>。

  • D 表示测量值的维度
  • E 表示测量值的数据类型
  • VertexXi，VertexXj 分别表示不同顶点的类型。（一元边只有一个顶点）

例如，BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>是个二元边。2是说测量值是2维的，也就是图像的像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap。

  • read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以

  • computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差

  • linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian。

重要的成员变量：

  • _measurement：存储观测值

  • _error：存储computeError() 函数计算的误差

  • _vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int有关（0 或1）

  • setId(int)： 来定义边的编号（决定了在H矩阵中的位置）

  • setMeasurement(type) 函数来定义观测值

  • setVertex(int, vertex) 来定义顶点

  • setInformation() 来定义协方差矩阵的逆

最重要的就是computeError()，linearizeOplus()两个函数了。如果我们没有给出雅可比，g2o也会进行数值求导，但是会比重写的雅可比慢。 雅克比矩阵的维度为 误差维度 × 优化变量的维度 ，两个函数一般是这样的形式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
virtual void computeError() override
{
  // ...
    _error = _measurement - Something;
}
virtual void linearizeOplus() override
{
    _jacobianOplusXi(pos, pos) = something;
    // ...

    /*
    _jocobianOplusXj(pos, pos) = something;
    ...
    */
}

雅格比矩阵要针对每个顶点求解，上面说的维度是每个顶点求解的维度。比如边的类型继承
1
2
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>

由于是一元边，那么只针对一个顶点求解，误差维度是边的第一个模板类型2，优化变量的维度是顶点的第一个模板参数6.

TEB算法中的class EdgeVelocity : public BaseTebMultiEdge<2, double>，是三元边，两个VertexPose顶点，类型PoseSE2。顶点VertexTimeDiff，类型double。对于前者，雅格比矩阵是 2x3，对后者是 2x1，也就是如下

1
2
3
_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT

对于边class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>，这是二元边，雅格比包含两部分，维度分别为 2x3 和 2x6. 后一个顶点的类型是李群。可以这样写

1
2
3
4
5
// 相对 _jacobianOplus[2] 的另一种写法
_jacobianOplusXi(0,0) =
......
_jacobianOplusXj(0,0) = 
......

removeEdge()

removeEdge 和 removeVertex只有在ORB-SLAM中看到。

After running optimize() once, I am finding a high number of outliers. Is there any way to delete a single edge out of the graph and run optimize again, or do I have to construct it again?

The method removeEdge() removes the edge from a graph and unlinks it from all of the vertices it was attached to. After you call it on your edges, I think you need to call initializeOptimization() to reset all of g2o’s internal data structures to the new graph configuration. You should then be able to call optimize()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
// 剔除一些误差大的边
// Check inliers
int nBad=0;
for(size_t i=0; i<vpEdges12.size();i++)
{
    g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
    g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
    if(!e12 || !e21)
        continue;

    if(e12->chi2()>th2 || e21->chi2()>th2)
    {
        size_t idx = vnIndexEdge[i];
        vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        optimizer.removeEdge(e12);
        optimizer.removeEdge(e21);
        vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
        vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
        nBad++;
    }
}

setLevel(int level)

也是在ORB-SLAM中用到

优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化

setLevel(int ) is useful when you call optimizer.initializeOptimization(int ). If you assign initializeOptimization(0), the optimizer will include all edges up to level 0 in the optimization, and edges set to level >=1 will not be included

1
2
3
4
5
6
// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
// 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
if(e->chi2()>5.991 || !e->isDepthPositive())
{
    e->setLevel(1);// 不优化
}
范数及求导|数学基础

||x|| 是 1 范数

2范数其实就是：列向量 x, 2范数为

2范数为

对 x 求导

如果是求雅格比，

也就是只需求f(x)的雅格比，再转置。


再看矩阵的范数

  • 0 范数: 矩阵中非零元素的个数

  • 1 范数: 又称为 列和范数，将每列元素进行绝对值求和，取最大值

  • 范数: 又称为 列和范数，将每列元素进行绝对值求和，取最大值

  • 2 范数: 公式为 ， 其中 表示 的最大特征值。 2范数可以用来判断一个矩阵是不是病态矩阵

2范数是由向量范数诱导而来，F范数是直接定义，是两种不同的度量方式。F范数可以比较真实的矩阵和估计的矩阵值之间的误差。

C++的各种报错和报警|C++C++ 面向对象

还是Qt的环境好，很多时候，不用编译直接就提示有错误。

  • 报错invalid new-expression of abstract class type

有的编译器也会报错 error: allocating an object of abstract class type ‘child’。说明基类中有纯虚函数没有实现，必须在派生类实现所有纯虚函数，不使用派生类时是看不出来的。

  • marked ‘override’, but does not override
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
class Parent
{
public:
  virtual std::string getName1(int x)
  {
    return "Parent";
  }

  virtual std::string getName2(int x)
  {
    return "Parent";
  }
};

class Child : public Parent
{
public:
  virtual std::string getName1(double x) override
  {
    return "Sub";
  }
  virtual std::string getName2(int x) const override
  {
    return "Sub";
  }
};

派生类的同名函数和基类的参数类型不同，而且多了const。如果没有override，那么是派生类新加的函数，但是添加override，编译器认为基类有同名函数，但是没发现，所以报错。

如果是重写基类方法，建议使用override标识符，让编译器检查出可能的错误，从而避免无意的错误。

PCL中的NDT|PCL点云
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/transforms.h>
#include <pcl/console/time.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>      //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>   //滤波类头文件  （使用体素网格过滤器处理的效果比较好）
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <boost/thread/thread.hpp>

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> PointCloud;
typedef pcl::PointCloud<PointType>::Ptr PointCloudPtr;
typedef pcl::PointCloud<PointType>::ConstPtr PointCloudConstPtr;

int main (int argc, char** argv)
{
    ros::init(argc, argv, "test_ndt");
    ros::NodeHandle nh;

    PointCloudPtr target_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/user/test_nodes/src/test_pcl/room_scan1.pcd", *target_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << target_cloud->size() << " points" << endl; 

    PointCloudPtr input_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/user/test_nodes/src/test_pcl/room_scan2.pcd", *input_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << input_cloud->size() << " points" << endl;

    PointCloudPtr filtered_cloud(new PointCloud);
    pcl::VoxelGrid<PointType> filter;
    filter.setLeafSize(0.1, 0.1, 0.1);
    filter.setInputCloud(input_cloud);
    filter.filter(*filtered_cloud);
    cout << "after filtered, input cloud has "<< filtered_cloud->size() << " points" << endl;
    
    pcl::NormalDistributionsTransform<PointType, PointType>::Ptr ndt_ptr(new pcl::NormalDistributionsTransform<PointType, PointType>() );
    ndt_ptr->setMaximumIterations(35);
    ndt_ptr->setTransformationEpsilon(0.01);
    ndt_ptr->setStepSize(0.1);
    ndt_ptr->setResolution(0.1);
    ndt_ptr->setInputSource(filtered_cloud);
    ndt_ptr->setInputTarget(target_cloud);

    Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

    PointCloudPtr output_cloud(new PointCloud);
    ndt_ptr->align(*output_cloud, init_guess);
    cout << "output cloud points size: "<< output_cloud->size() << endl;
    cout << "ndt transform: "<< endl << ndt_ptr->getFinalTransformation() << endl;
    cout << "score: "<< ndt_ptr->getFitnessScore() << endl;
    
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt_ptr->getFinalTransformation() );
    pcl::io::savePCDFileASCII("/home/user/test_nodes/src/test_pcl/final.pcd", *output_cloud );

    // 初始化点云可视化对象
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
    viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色

    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
    target_color (target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "target cloud");

    // 对转换后的源点云着色 (green)可视化.
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
    output_color(output_cloud, 0, 0, 255);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");
    // 启动可视化
    viewer_final->addCoordinateSystem (1.0);  //显示XYZ指示轴
    viewer_final->initCameraParameters ();   //初始化摄像头参数

    // 等待直到可视化窗口关闭
    while (!viewer_final->wasStopped ())
    {
      viewer_final->spinOnce (101);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

三个迭代终止条件设置：

  1. setMaximumIterations(30): 设置最大的迭代次数。 一般来说在这个限制值之前，优化程序会在epsilon（最大容差）变换阀值下终止，添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
  2. setTransformationEpsilon: 终止条件设置最小转换差异，设置前后两次迭代的转换矩阵的最大容差epsilion，在变换中Epsilon参数分别从长度和弧度，定义了变换矢量[x, y, z, roll, pitch, yaw]的最小许可的递增量，一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，默认值为：0
  3. setEuclideanFitnessEpsilon: 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max

把setTransformationEpsilon的参数从0.01改为0.001，FitnessScore 只有细微变化，而且相对之前有时变大有时变小。但是改为0.1，FitnessScore与0.01时相比，也是有大有小。迭代次数不管怎么调整，FitnessScore毫无变化，可见确实是以epsilon为优先终止条件。

  • setStepSize(0.1): More-Thuente线搜索设置最大步长，即设置牛顿法优化的最大步长。当你靠近最优解时该算法会缩短迭代步长，在最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。 调试的经验 ：step的话可以先放大后变小。

  • setResolution(1.0);: 设置网格化时立方体的边长，网格大小设置在NDT中非常重要，太大会导致精度不高，太小导致内存过高，并且只有两幅点云相差不大的情况才能匹配。它需要足够大，每个体素包含与点有关的统计数据，平均值，协方差等。每个体素至少包含6个点，但小到足以唯一地描述环境。 调试的经验 ：resolution在追求精度的情况下最好与点密度相等或者低一个数量级,这里是激光点云，所以设了1米

  • getFitnessScore()：欧几里得适合度得分 FitnessScore ，该分数计算为从输出云到目标云中最近点的距离的平方。分数越大，准确率越低，配准效果也就越差。

align函数中，output_cloud保存变换后的input_cloud，它和target_cloud其实很相近了。但是它不能作为最终的源点云变换，因为上面对input_cloud进行了滤波处理，所以又经过pcl::transformPointCloud(*input_cloud, *output_cloud, ndt_ptr->getFinalTransformation() );获得未经滤波的output_cloud

注意的地方：

  • 开始做体素滤波，栅格太大，过滤太多，最后的output_cloud和target_cloud看着不太契合
  • 只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理


参考: 参考程序

深入分析 constraint_list 话题|激光SLAMCartographer源码解读

这个话题是显示所有的约束，包括同一轨迹和不同轨迹的Inter约束，Intra约束。

以Inter constraints, different trajectories 和 Inter residuals, different trajectories为例进行分析，内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
# 每个点的 z为0.0，省略
header: 
  seq: 0
  stamp: 
    secs: 1673860622
    nsecs: 104007813
  frame_id: "map"
ns: "Inter constraints, different trajectories"
# 12个点，有6个点相同或极为接近
points: 
  - 
    x: 2.93968786067
    y: -1.41820975755
  - 
    x: 3.44855066169
    y: -1.24014780956
  - 
    x: -0.000569545301176
    y: 0.00522600210993
  - 
    x: 3.4816963258
    y: -1.19978045681
  - 
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 3.78063457495
    y: -1.54584384415
  - 
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 4.61884003257
    y: -1.49287256215
  - 
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 4.6545497049
    y: -1.84995755765
  -
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 4.91335613134
    y: -2.16836009999
 

- 
header: 
  seq: 0
  stamp: 
    secs: 1673860622
    nsecs: 104007813
  frame_id: "map"
ns: "Inter residuals, different trajectories"
# 12个点，有些点很接近，结果构成的线很短
points: 
  - 
    x: 3.44855066169
    y: -1.24014780956
  - 
    x: 3.47573816704
    y: -1.1999840042
  - 
    x: 3.4816963258
    y: -1.19978045681
  - 
    x: 3.47896481246
    y: -1.20118000282
  - 
    x: 3.78063457495
    y: -1.54584384415
  - 
    x: 3.80065304682
    y: -1.55796489639
  - 
    x: 4.61884003257
    y: -1.49287256215
  - 
    x: 4.65790271882
    y: -1.44107978827
  - 
    x: 4.6545497049
    y: -1.84995755765
  - 
    x: 3.00991092288
    y: -2.53605574275
  - 
    x: 4.91335613134
    y: -2.16836009999
  - 
    x: 3.11523021674
    y: -2.61951559764

源码中可以看到Rviz显示的是LINE_LIST类型的visualization_msgs::Marker，这里要提一下Rviz显示的线类型，如下

LINE_LIST是两两连接而成的线段，在carto这里，点数一定为偶数。把上面消息内容，没两个点的坐标连线，就可以得到Rviz里的显示

黄线有6条，水绿线有2条比较长，仔细还能看到3条很短的，最后一条在坐标系附近，肉眼已经看不到。Inter约束和residual个数相同，有一个共同的端点原因可以看源码。

看MapBuilderBridge::GetConstraintList源码部分有些跳过了

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
const auto trajectory_node_poses =
    map_builder_->pose_graph()->GetTrajectoryNodePoses();
const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
const auto constraints = map_builder_->pose_graph()->constraints();
for (const auto& constraint : constraints)
{
    const auto submap_it = submap_poses.find(constraint.submap_id);
    if (submap_it == submap_poses.end()) {
      continue;
    }
    const auto& submap_pose = submap_it->data.pose;
    const auto node_it = trajectory_node_poses.find(constraint.node_id);
    if (node_it == trajectory_node_poses.end()) {
      continue;
    }
    const auto& trajectory_node_pose = node_it->data.global_pose;
    const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;

    // Inter constraints, different trajectories  线段的两个端点
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(submap_pose.translation()));
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    // Inter residuals, different trajectories  线段的两个端点
    residual_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    residual_marker->points.push_back(
        ToGeometryMsgPoint(trajectory_node_pose.translation()));

从最后可以看出，共同的端点是constraint_pose，map坐标系下。

ceres 5 核函数|SLAM工具

核函数：某个数据点的残差过大的时候，降低其权重。以曲线拟合为例，

我们拟合点的时候，显然希望红线而不是黄线，outlier可以抛弃。

残差大于0.1则降低其权重
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);

CMake 各种报错报警|cmake/qmake
  • integer_sequence’ is not a member of ‘std’

编译工程时报错 integer_sequence’ is not a member of ‘std’
CMake中添加 set(CMAKE_CXX_FLAGS "-std=c++14")

  • std::make_unique的报错

在CMake3.16版本编译std::make_unique部分的代码，没有报错。在3.10版本上编译却报错了，在CMakeLists.txt加入set(CMAKE_CXX_STANDARD 14)后才成功。

  • CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): Compatibility with CMake < 2.8.12 will be removed from a future version of CMake

我升级CMake后出现的报警，虽然不影响，但是一旦工程多了以后，这个报警会很烦人。只需要修改cmake_minimum_required (VERSION 3.19.0)

但是有时CMakeLists太多了，不可能一一去修改，使用sed命令集体修改：

1
sed -i "s/2.8.3/3.19/g" `grep 2.8.3 -rl . --include="*.txt" `

这个命令不要滥用，否则可能更改过多

  • 涉及PCL的一个警告
1
2
3
4
5
CMake Warning (dev) at /usr/local/share/cmake-3.17/Modules/FindPackageHandleStandardArgs.cmake:272 (message):
  The package name passed to `find_package_handle_standard_args` (PCL_KDTREE)
  does not match the name of the calling package (PCL).  This can lead to
  problems in calling code that expects `find_package` result variables
  (e.g., `_FOUND`) to follow a certain pattern.

在find_package(PCL REQUIRED)之前加上

1
2
3
if(NOT DEFINED CMAKE_SUPPRESS_DEVELOPER_WARNINGS)
    set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS 1 CACHE INTERNAL "No dev warnings")
endif()

  • No rule to make target

有时明明写好了，但编译会出现报错，看上去是CMakeLists中没有编译规则

1
2
3
4
5
6
make[2]: *** No rule to make target 'package/CMakeFiles/test_bin.dir/build'。 停止。
CMakeFiles/Makefile2:3192: recipe for target 'package/CMakeFiles/test_bin.dir/all' failed
make[1]: *** [package/CMakeFiles/test_bin.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed

此时再重新编译仍然报错，只要把CMakeLists改一下，再编译就通过了

  • CMakeCache 报错

执行编译时报错:

1
CMake Error: The current CMakeCache.txt directory /home/user/common/build/CMakeCache.txt is different than the directory /home/user/space/build where CMakeCache.txt was created. This may result in binaries being created in the wrong place. If you are not sure, reedit the CMakeCache.txt

将当前工程的CMakeCache全删除再编译
1
rm $(find -name *.txt | grep CMakeCache)

  • unrecognized command line option -msse

这是因为在arm平台上不存在SSE指令集的，在x86平台才会有，因此需要在CMakLists文件中把—msse字样的都注释掉