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字样的都注释掉

纯定位的优化改进|激光SLAMCartographer优化
abstract Welcome to my blog, enter password to read.
Read more
landmark的添加残差块|激光SLAMCartographer源码解读
1
2
3
4
5
6
7
8
problem->AddResidualBlock(
    LandmarkCostFunction2D::CreateAutoDiffCostFunction(
        observation, prev->data, next->data),

    new ceres::HuberLoss(huber_scale), 
    prev_node_pose->data(),  next_node_pose->data(), 
    C_landmarks->at(landmark_id).rotation(),
    C_landmarks->at(landmark_id).translation() );

CreateAutoDiffCostFunction

1
2
3
4
5
6
7
8
9
10
11
12
13
14
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const LandmarkObservation& observation, const NodeSpec2D& prev_node,
      const NodeSpec2D& next_node)
{
    return new ceres::AutoDiffCostFunction<
        LandmarkCostFunction2D,
        6 /* residuals，operator() return的是6维向量 */,
                    // 所估计变量的维度
        3 /* previous node pose variables (x,y,theta) */, 
        3 /* next node pose variables  (x,y,theta)  */,
        4 /* landmark rotation variables  */,
        3 /* landmark translation variables */>(
        new LandmarkCostFunction2D(observation, prev_node, next_node));
}

operator

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
  template <typename T>
  bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
                  const T* const landmark_rotation,
                  const T* const landmark_translation,
                  T* const e) const
{
    const std::tuple<std::array<T, 4>, std::array<T, 3>>
        interpolated_rotation_and_translation = InterpolateNodes2D(
            prev_node_pose, prev_node_gravity_alignment_, next_node_pose,
            next_node_gravity_alignment_, interpolation_parameter_);

    const std::array<T, 6> error = ScaleError(
        ComputeUnscaledError(
            landmark_to_tracking_transform_,
            std::get<0>(interpolated_rotation_and_translation).data(),
            std::get<1>(interpolated_rotation_and_translation).data(),
            landmark_rotation, landmark_translation),
        translation_weight_, rotation_weight_);

    std::copy(std::begin(error), std::end(error), e);
    return true;
}

ComputeUnscaledError

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
template <typename T>
static std::array<T, 6> ComputeUnscaledError(
    const transform::Rigid3d&  relative_pose, 
    const T* const start_rotation,  const T* const start_translation, 
    const T* const end_rotation,    const T* const end_translation )
{
	// 四元数共轭，在这里就是逆
  const Eigen::Quaternion<T> R_i_inverse( start_rotation[0],
                                         -start_rotation[1],
                                         -start_rotation[2],
                                         -start_rotation[3] );
    // 平移的差
  const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0],
                                     end_translation[1] - start_translation[1],
                                     end_translation[2] - start_translation[2]);
  const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;

  const Eigen::Quaternion<T> h_rotation_inverse =
      Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], 
                          -end_rotation[2], -end_rotation[3]) *
      Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
                           start_rotation[2], start_rotation[3]);

  const Eigen::Matrix<T, 3, 1> angle_axis_difference =
      transform::RotationQuaternionToAngleAxisVector(
          h_rotation_inverse * relative_pose.rotation().cast<T>());

  return {  {T(relative_pose.translation().x()) - h_translation[0],
             T(relative_pose.translation().y()) - h_translation[1],
             T(relative_pose.translation().z()) - h_translation[2],
           angle_axis_difference[0], 
           angle_axis_difference[1],
           angle_axis_difference[2]}  };
}

relative_pose 是 landmark_to_tracking_transform，landmark和tracking坐标系的相对位姿，在SensorBridge::HandleLandmarkMessage中获得

landmark作为独立变量进入Pose Graph的稀疏优化，它增加的优化维度是 landmarks个数 * 每个landmark的参数个数，不要使用太多landmark

ScaleError一看就是残差乘以权重

1
2
3
4
5
6
7
8
9
10
11
template <typename T>
std::array<T, 3> ScaleError( const std::array<T, 3>& error,
                             double translation_weight,
                             double rotation_weight )
{
  return {{
      error[0] * translation_weight,
      error[1] * translation_weight,
      error[2] * rotation_weight
  }};
}

landmark的后端处理|激光SLAMCartographer源码解读

加入landmark后，Pose Graph有了不同的node和edge，也就是landmark也作为node，landmark-pose之间的 edge是新的一种edge，即在pose对landmark的观测

1
2
3
4
5
6
7
8
9
10
11
12
13
struct LandmarkNode {
    struct LandmarkObservation {
      int trajectory_id;
      common::Time time;
      transform::Rigid3d landmark_to_tracking_transform;
      double translation_weight;
      double rotation_weight;
    };
    // 同一时刻，可能观测到多个landmark数据
    std::vector<LandmarkObservation> landmark_observations;
    absl::optional<transform::Rigid3d> global_landmark_pose;
    bool frozen = false;
  };

landmark是应用于后端，不是实时的，应当调大权重让carto更相信它的结果

添加数据部分

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
void PoseGraph2D::AddLandmarkData(int trajectory_id,
                                  const sensor::LandmarkData& landmark_data)
{
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    if (CanAddWorkItemModifying(trajectory_id)) {
      for (const auto& observation : landmark_data.landmark_observations)
      {
      	          			// initial landmark_nodes
          // const transform::Rigid3d global_pose =
          // observation.landmark_to_map_transform;
      #if 0
      data_.landmark_nodes的类型是 std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
      #endif
        data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
            PoseGraphInterface::LandmarkNode::LandmarkObservation{
                trajectory_id, landmark_data.time,
                observation.landmark_to_tracking_transform,
                observation.translation_weight, observation.rotation_weight} );

          // data_.landmark_nodes[observation.id].global_landmark_pose = global_pose;
      }
    }
    // optimization_problem_->new_landmark_add_ = true;
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

这里就是把观测到的landmark信息都保存到data_.landmark_nodes，最后返回的标识也是不优化

后端优化的流程和其他传感器数据一样:
PoseGraph2D::HandleWorkQueue —— PoseGraph2D::RunOptimization —— OptimizationProblem2D::Solve(data_.constraints, GetTrajectoryStates(), data_.landmark_nodes) —— AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, &problem, options_.huber_scale() )

OptimizationProblem2D::Solve部分：

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
  std::set<int> frozen_trajectories;
  for (const auto& it : trajectories_state)
  {
    if (it.second == PoseGraphInterface::TrajectoryState::FROZEN)
    {
      frozen_trajectories.insert(it.first);
    }
  }

  ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);

  // Set the starting point.  TODO(hrapp): Move ceres data into SubmapSpec.
  // ceres需要的是double指针，std::array 能转成原始指针的形式
  MapById<SubmapId, std::array<double, 3>> C_submaps;
  MapById<NodeId, std::array<double, 3>> C_nodes;
  std::map<std::string, CeresPose> C_landmarks;
  bool first_submap = true;


  // 将需要优化的  子图位姿  设置为优化参数
  for (const auto& submap_id_data : submap_data_) {
    // submap_id的轨迹是否是冻结的
    const bool frozen =
        frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
        // 将子图的global_pose放入 C_submaps
    C_submaps.Insert(submap_id_data.id,
                     FromPose(submap_id_data.data.global_pose));
    // c++11中，std::array::data()返回指向数组对象第一个元素的指针
    problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);

    // 第一个子图或冻结，不优化子图位姿。 也就是不优化初值
    if (first_submap || frozen) {
      first_submap = false;
      // Fix the pose of the first submap or all submaps of a frozen
      // trajectory.
      problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
    }
  }

  // 需要优化的  节点位姿  设置为优化参数，与上面的子图优化大致相同
  for (const auto& node_id_data : node_data_)
  {
    const bool frozen =
        frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
    problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
    if (frozen)
    {
      // 这里的第一个节点也要参与优化，跟子图的不同了
      problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
    }
  }
				/* ...... */
// 第2种残差：landmark与landmark数据插值出来的节点相对位姿的差值
AddLandmarkCostFunctions(landmark_nodes, node_data_, 
						&C_nodes, &C_landmarks,
                        &problem, options_.huber_scale()  );

landmark数据 与 通过2个节点位姿插值出来的相对位姿 的差值作为残差项

AddLandmarkCostFunctions

最外层两个循环是遍历landmark节点和遍历每个节点的观测，也就是说可能会同时看到多个landmark。直接看循环里面的内容

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
const std::string& landmark_id = landmark_node.first;
// 轨迹中第一个 node_data
const auto& begin_of_trajectory =
    node_data.BeginOfTrajectory(observation.trajectory_id);
// 如果 landmark observation was made before the trajectory was created
if (observation.time < begin_of_trajectory->data.time) {
    continue;
}
/* 以下语句Find the trajectory nodes before and after the landmark observation */

// 找到在landmark观测时间后的第一个节点
auto next = 
    node_data.lower_bound(observation.trajectory_id, observation.time);
/*  The landmark observation was made, but the next trajectory node has
    not been added yet. 即next已经是轨迹最后一个节点  */
if (next == node_data.EndOfTrajectory(observation.trajectory_id)  ) {
    continue;
}
// 如果是刚开始的node data
if (next == begin_of_trajectory) {
  next = std::next(next);
}
// 这里的pre配合next是为了获取两个位置，找到landmark观测时间的前一个节点
auto prev = std::prev(next);

// 根据两个索引，获取两个节点位姿
// Add parameter blocks for the landmark ID if they were not added before
std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);
std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);

根据landmark观测的时间，找到观测前后的节点，获取位姿

1
2
3
4
5
6
7
8
9
// 如果landmark_id 不存在于 C_landmarks，
if (!C_landmarks->count(landmark_id)){
  // landmark如果已经优化，则 global_landmark_pose 有值
  // 否则根据时间插值计算一个位姿
  const transform::Rigid3d starting_point =
      landmark_node.second.global_landmark_pose.has_value()
          ? landmark_node.second.global_landmark_pose.value()
          : GetInitialLandmarkPose(observation, prev->data, next->data,
                                   *prev_node_pose, *next_node_pose);

开始C_landmarks当然为空，只有在优化完成后，这里才存在，即 RunOptimization 的最后会添加: data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;，顺着上面的调用路线就能发现

GetInitialLandmarkPose

这里用了一堆数据类型转换，以及四元数、旋转向量、欧拉角之间的转换。还重新实现了slerp函数SlerpQuaternions，因为Eigen的slerp与Ceres的不兼容。
过程


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
  C_landmarks->emplace(
      landmark_id,
      // 四元数不支持广义加法，使用QuaternionParameterization 
      CeresPose(starting_point, nullptr /* translation_parametrization */,
                absl::make_unique<ceres::QuaternionParameterization>(),
                problem)  );

  // Set landmark constant if it is frozen. 不参与优化
  // 这里容易记不清，frozen 定义在  struct LandmarkNode
  if (landmark_node.second.frozen) {
    problem->SetParameterBlockConstant(
        C_landmarks->at(landmark_id).translation());

    problem->SetParameterBlockConstant(
        C_landmarks->at(landmark_id).rotation());
  }
}

CeresPose的构造函数里就两行

1
2
3
4
problem->AddParameterBlock(data_->translation.data(), 3,
                         translation_parametrization.release());
problem->AddParameterBlock(data_->rotation.data(), 4,
                         rotation_parametrization.release());

对于平移，无需ceres::LocalParameterization


添加残差块

1
2
3
4
5
6
7
8
problem->AddResidualBlock(
    LandmarkCostFunction2D::CreateAutoDiffCostFunction(
        observation, prev->data, next->data),

    new ceres::HuberLoss(huber_scale), 
    prev_node_pose->data(),  next_node_pose->data(), 
    C_landmarks->at(landmark_id).rotation(),
    C_landmarks->at(landmark_id).translation() );

if (!C_landmarks->count(landmark_id))已经结束，添加残差块。

Solve 和 RunOptimization 的最后处理

Solve的最后

1
2
3
4
for (const auto& C_landmark : C_landmarks) {
	// first 是 landmark_id
	landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}

这个C_landmarks还是上面那个，ToRigid()当然是优化后的landmark全局位姿了

在RunOptimization的最后

1
2
3
4
5
6
// 遍历成员变量 landmark_data_，它在 Solve 函数的最后赋值
for (const auto& landmark : optimization_problem_->landmark_data() )
{
	 // first 是 landmark_id
  data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
}

optimization_problem_->landmark_data()返回的就是上面的landmark_data_，说白了，这里就是把优化后的landmark数据从OptimizationProblem2D层传回PoseGraph2D层

landmark的前端处理|激光SLAMCartographer源码解读

landmark就是添加了一个观测，让定位更鲁棒。如果观测是空的，应当发布空的 landmark messages to unblock the queue. 对于fixed frame poses (GPS)也适用。

前端的处理

node.cc 层

1
2
3
4
// landmark_poses_list 话题
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);

发布话题：

1
2
3
4
5
6
7
8
void Node::PublishLandmarkPosesList(const ::ros::WallTimerEvent& unused_timer_event)
{
  if (landmark_poses_list_publisher_.getNumSubscribers() > 0)
  {
      absl::MutexLock lock(&mutex_);
      landmark_poses_list_publisher_.publish(map_builder_bridge_.GetLandmarkPosesList() );
  }
}

处理传感器数据

1
2
3
4
5
6
7
8
9
10
11
void Node::HandleLandmarkMessage(
    const int trajectory_id, const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg)
{
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) 
      return;
  CHECK(msg->landmarks.size() != 0) << "No Landmarks !";

  map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLandmarkMessage(sensor_id, msg);
}

sensor_bridge.cc 层

用到的类型

1
2
3
4
5
6
7
8
9
10
11
12
struct LandmarkData {
  common::Time time;
  std::vector<LandmarkObservation> landmark_observations;
};

struct LandmarkObservation
{
  std::string id;
  transform::Rigid3d landmark_to_tracking_transform;
  double translation_weight;
  double rotation_weight;
};

如果是二维码，就可以将识别到的ID传进去，landmark的观测是相对于 tracking 坐标系的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void SensorBridge::HandleLandmarkMessage(
    const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg)
{
  auto landmark_data = ToLandmarkData(*msg);
  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
  // 把相机对landmark的观测，转换到 tracking 坐标系
  if (tracking_from_landmark_sensor != nullptr)
  {
      for (auto& observation : landmark_data.landmark_observations)
      {
        observation.landmark_to_tracking_transform =
            *tracking_from_landmark_sensor *
            observation.landmark_to_tracking_transform;
      }
  }
  // 进入传感器数据融合阶段
  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}

简单的类型转换而已

1
2
3
4
5
6
7
8
9
10
11
12
13
14
LandmarkData ToLandmarkData(const LandmarkList& landmark_list)
{
    LandmarkData landmark_data;
    landmark_data.time = FromRos(landmark_list.header.stamp);
    for (const LandmarkEntry& entry : landmark_list.landmark)
    {
        landmark_data.landmark_observations.push_back(
        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
         entry.translation_weight, 10, entry.type});
         //LOG(INFO) << "find landmark x: " << entry.tracking_from_landmark_transform.position.x;
         //LOG(INFO) << "find landmark y: " << entry.tracking_from_landmark_transform.position.y;
    }
    return landmark_data;
}

数据融合阶段

CollatedTrajectoryBuilder类

1
2
3
4
5
6
7
8
9
10
11
12
void AddSensorData(const std::string& sensor_id,
                   const sensor::LandmarkData& landmark_data) override
{
    if (collate_landmarks_)
    {
      AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
      return;
    }
    // wrapped_trajectory_builder_ 赋值在 CollatedTrajectoryBuilder 构造函数
    // 应当为map_builder_->GetTrajectoryBuilder(trajectory_id)
    wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}

  • collate_landmarks的设置问题

在CollatedTrajectoryBuilder的构造函数里体现，注意设置为on或off，不是true, false

1
2
3
4
5
if (sensor_id.type == SensorId::SensorType::LANDMARK &&
    !collate_landmarks_)
{
    continue;
}

如果提供landmark的观测不低于10 Hz，那么可以设置TRAJECTORY_BUILDER.collate_landmarks = on. Cartographer将deterministically运行(对于给定的bag, 使用offline node每次获得的结果是一样的). 如果collate_landmarks = off， landmark observations将跳过sensor queue (so they do not block it if they are sparse) and are injected directly into the pose graph, which is not deterministic.

但是看到另一种说法，在 #1224更新版本，作者提出如果有landmark，collate_landmarks就要为 false。这个再观察

接下来进入GlobalTrajectoryBuilder

1
2
3
4
5
void AddSensorData(const std::string& sensor_id,
                   const sensor::LandmarkData& landmark_data) override
{
   pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
}

这里可以看到landmark数据直接进后端，前端没有它的事了

旷视的CamLaserCalibraTool|激光SLAM其他

看到网上有些人说标定结果不错，但我试了很多次，开始还能出运行结果，但是和实际差太多，后来换了个相机，几乎不能运行成功了。kalibr图案、棋盘格、apriltag都试过了，代码调试了很多地方，都不顺利，看github也有很多issue未解决，先搁置了

配置

laser_filter的配置filter.yaml修改如下

1
2
3
4
5
6
7
8
9
10
11
12
scan_filter_chain:
- name: angle
  type: laser_filters/LaserScanAngularBoundsFilter
  params:
    lower_angle: -0.6
    upper_angle: 0.6
- name: range
  type: laser_filters/LaserScanRangeFilter
  params:
    use_message_range_limits: false
    lower_threshold: 0.5
    upper_threshold: 2.2

配置文件中的tag_spacing = 0.3并不是tag_spacing的实际距离大小，而是比例：8.8x0.3=2.64，因此这里的tag_spacing是一个固定值为0.3，不用修改！

录制bag要包括雷达scan和图像

1
rosbag record scan_filtered /camera_node/image_raw -O pinhole.bag

roslaunch lasercamcal_ros kalibra_apriltag.launch会把检测到每一帧中的april的信息保存到apriltag_pose.txt文件中，作者录制的数据包有1527 msgs，最终也是每一帧都检测到了，我录制的bag有时会有一些帧检测不到，似乎不是什么问题。

作者也是没有对录制的包直接做数据时间同步的，相机的帧率大概是2D激光雷达的二倍

移动标定板

要求是：标定板在激光正前方120°范围内，并且激光前方2m范围内只存在一个连续的直线线段，所以请在空旷的地方采集数据，不然激光数据会提取错误。需要充分旋转 roll 和 pitch。更直白一点，假设在长方形的标定板中心画了个十字线，那请绕着十字线的两个轴充分旋转，比如绕着竖轴旋转，然后还要绕着横轴旋转。在运行offline程序时，程序会将认为正确的直线会显示为红色。

激光线要能落在标定板上。不断调整标定板姿态，每换一个姿态(绕Roll, Pitch, Yaw旋转，移动位置)，请保持静止 2 秒以上，采集 10 个姿态左右的数据（当然越多越好）。用 rosbag 记录所有的图像和激光数据， 标定工具会自动检测你保持静止时刻的数据，并用来标定。

1
2
3
4
5
6
7
--------- start AutoGetLinePts ---------
id_left: 182, dist left: 1414
id_right: 0, dist right: 1

currentPt: 0  nextPt: 3
d1: 1.085,  d2: 1414.21
d2: 1000  1000

这里的1000,1414其实就是对应scan的range为nan的情况，在if(d1 < range_max && d2 < range_max)里过滤掉了，不用管了

失败的情况

Valid Calibra Data too little

运行roslaunch lasercamcal_ros calibra_offline.launch，结果OpenCV窗口的标定板对应线段不是红色，也没有results.yaml
终端出现Valid Calibra Data too little

查代码发现points = AutoGetLinePts(Points);没有检测到直线。接着查发现问题在selectScanPoints.cpp中的

1
2
3
4
5
6
7
8
9
 // 至少长于 20 cm, 标定板不能距离激光超过2m, 标定板上的激光点超过 50 个
if(dist.head(2).norm() > 0.2
   && points.at(seg.id_start).head(2).norm() < 2
   && points.at(seg.id_end).head(2).norm() < 2
   && seg.id_end-seg.id_start > 50   ) 
{
    seg.dist = dist.head(2).norm();
    segs.push_back(seg);
}

有时第一项和最后一项不符合；有时第一项符合，但中间两项又不符合；第四项大部分情况是3. 常见的四个参数如下
1
2
3
4
5
6
7
8
9
0.100479
1.294
1.218
3

0.201422
2.357
2.513
3

看来是不符合只有一个连续直线这条要求，后来又试了一次，虽然出现了红色直线，但是错的
错误的结果

最后终于明白了，源码中的参数都是作者的雷达相关的，需要根据自己雷达调整。int delta = 80/0.3;的两个参数根据雷达角分辨率调整。50降为18，这样就成功了，int skip = 3;也可能要调整。

但是常常不管怎么调参数，仍然可用数据太少

越界

运行calibra_offline.launch得到的bug

1
2
3
4
Load apriltag pose size: 582
terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 18446744073709551615) >= this->size() (which is 57)
[lasercamcal_ros-1] process has died

有时连录几次bag，都出现这种情况，估计是某个地方在for循环里push_back导致

结果不可观

计算初值阶段

1
2
3
4
5
6
7
8
9
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 Notice Notice Notice: system unobservable !!!!!!!
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

------- Closed-form solution Tlc: -------
   1    0    0 -nan
   0    1    0 -nan
   0    0    1  nan
   0    0    0    1