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就是添加了一个观测，让定位更鲁棒。

In order to make Cartographer calculations deterministic we strictly time order sensor data using the collator. But you only know in what order to publish data when you have already seen data from all other sources. Sparse sources pose a problem to that strategy.

如果观测是空的，应当发布空的 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;
};

landmark的观测是相对于 tracking frame

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

tf(三) lookTransform和齐次变换矩阵的关系推导

建立tf树 A—->B—->C，求 C—->A变换

1
2
rosrun tf static_transform_publisher 0.169, 0.035, -0.125,   -1.57, 0, -1.57  A  B 100
rosrun tf static_transform_publisher   0,   0.015,     0,    -1.57, 0, -1.57  B  C 100

平时使用的tf是tf::Transformer::lookupTransform(target_frame, source_frame)，表示的是 source_frame ---> target_frame的变换，获得的位姿关系是子在父坐标系中，所以是lookupTransform("A", "B")，根据《SLAM十四讲》63页的表示方法，写成
，也就是父子顺序。 因此 C—->A变换是

代码

CMake关键部分：

1
2
3
4
5
6
7
find_package(catkin REQUIRED COMPONENTS
  eigen_conversions
  roscpp
  tf
  pcl_ros
)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)

c++关键部分
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
#include <ros/ros.h>
#include <Eigen/Dense>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>

tf::TransformListener listener;

ros::Rate rate(10.0);
while (node.ok())
{
	//创建一个StampedTransform对象存储变换结果数据
	tf::StampedTransform ab;
	tf::StampedTransform bc;
	tf::StampedTransform tfAC;
	try{
	    listener.lookupTransform("A", "B",
	                             ros::Time(0), ab);
	    listener.lookupTransform("B", "C",
	                             ros::Time(0), bc);
	    listener.lookupTransform("A", "C",
	                             ros::Time(0), tfAC);
	    // 输出 lookupTransform 输出的 C--->A 变换
	    cout << "X: "<< tfAC.getOrigin().getX() << " Y: "<< tfAC.getOrigin().getY()
	            <<"  Z: "<< tfAC.getOrigin().getZ()<< endl;
	    cout << "quaternion: "<< tfAC.getRotation().x() <<" " << tfAC.getRotation().y()<<
	            " " << tfAC.getRotation().z() << " " << tfAC.getRotation().w() << endl;

    Eigen::Matrix4f AB, BC, AC;
    // 使用 pcl_ros 直接得到 4x4的齐次变换矩阵
    pcl_ros::transformAsMatrix(ab, AB);   
    pcl_ros::transformAsMatrix(bc, BC);
    std::cout << "AB:"  <<std::endl;
    std::cout << AB  <<std::endl;
    std::cout << "BC:"  <<std::endl;
    std::cout << BC  <<std::endl;
    std::cout << "AC:"  <<std::endl;
    AC = AB * BC;
    std::cout << AC  <<std::endl;
    cout << "-------------------------------------" << endl;
  }
  catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
    continue;
  }
}

可以看到AC的右侧平移部分和tfAC.getOrigin()部分相同

tf::lookupTransform的源码一直追溯到tf2_ros::Buffer::lookupTransform，源码在f2/src/buffer_core.cpp，注意其中的accum函数和BufferCore::walkToTopParent函数

tf_eigen

tf_eigen.h提供了不少很有用的函数，用于tf和Eigen之间的矩阵和向量互相转换。使用前

1
2
3
4
find_package(catkin REQUIRED COMPONENTS
  eigen_conversions
  tf_conversions
)

头文件#include <tf_conversions/tf_eigen.h>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
// Converts an Eigen Quaternion into a tf Matrix3x3.
void  tf::matrixEigenToTF (const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
  // Converts a tf Matrix3x3 into an Eigen Quaternion.
void  tf::matrixTFToEigen (const tf::Matrix3x3 &t, Eigen::Matrix3d &e)

  // Converts an Eigen Affine3d into a tf Transform.
void  tf::poseEigenToTF (const Eigen::Affine3d &e, tf::Pose &t)
  // Converts a tf Pose into an Eigen Affine3d.
void  tf::poseTFToEigen (const tf::Pose &t, Eigen::Affine3d &e)
  
// Converts an Eigen Quaternion into a tf Quaternion.
void  tf::quaternionEigenToTF (const Eigen::Quaterniond &e, tf::Quaternion &t)
  // Converts a tf Quaternion into an Eigen Quaternion.
void  tf::quaternionTFToEigen (const tf::Quaternion &t, Eigen::Quaterniond &e)
  
  // Converts an Eigen Affine3d into a tf Transform.
void  tf::transformEigenToTF (const Eigen::Affine3d &e, tf::Transform &t)
  // Converts a tf Transform into an Eigen Affine3d.
void  tf::transformTFToEigen (const tf::Transform &t, Eigen::Affine3d &e)

  // Converts an Eigen Vector3d into a tf Vector3.
void  tf::vectorEigenToTF (const Eigen::Vector3d &e, tf::Vector3 &t)
   // Converts a tf Vector3 into an Eigen Vector3d.
void  tf::vectorTFToEigen (const tf::Vector3 &t, Eigen::Vector3d &e)
标定雷达和相机的外参|激光SLAM其他

粗标定

realsense和雷达外参的粗标定，也就是laser坐标系和camera_color_optical_frame之间的相对位姿，但如果realsense是低着头，竖直和横向的相对位姿的测量误差就大了。

使用realsense检测贴在墙上的二维码，获得距离，同时使用雷达检测到墙的距离，让雷达尽量正对二维码位置。

雷达到前方距离: 1.288，对应realsense的z值: 1.119，二者差 0.169
用直尺测量高度的差: 0.125
横向: realsense在雷达左侧 0.035

如果laser为父坐标系，二者的变换矩阵为

但realsense的坐标系体系是 camera_link —-> camera_color_frame —-> camera_color_optical_frame，其中

1
2
3
4
5
camera_link camera_color_optical_frame
- Translation: [0.000, 0.015, 0.000]
- Rotation: in Quaternion [-0.498, 0.502, -0.498, 0.502]
            in RPY (radian) [-1.571, 0.008, -1.570]
            in RPY (degree) [-89.992, 0.476, -89.975]

所以不能直接建立laser坐标系和camera_color_optical_frame的父子关系，要建立laser和camera_link的。

使用变换矩阵的计算，已知 ， 。 计算

结果转为tf格式，应当为 <node pkg="tf" type="static_transform_publisher" name="laser_camera_broadcaster" args="0.169 0.02 -0.125 0 0 0 laser camera_link 100"/>

向量，平面和坐标系变换|数学基础

向量

  • 标准化后的向量点乘得到的值为夹角的余弦。如果得到 -1, 0, -1，便可知道两个向量方向的关系是相反, 垂直, 相同

  • 向量叉乘后得到的向量和原先两个向量垂直，也就是法向量

  • 向量叉乘后得到的向量的模，其值为两个向量构成的三角形的面积的二倍。

  • 叉乘的一些性质


用行列式的表示方法观察，不用计算就能看出前三条。

平面的一般式

高中学过的表示方法为 ，法向量为 。这里涉及到了归一化问题，两边可以乘以系数，还是同一个平面，也可以说法向量是不唯一的。但如果给出了一般式 ，就说法向量为

空间内一点 到平面的距离是 ，如果已经归一化，那么分母就是 1

法向量可以看做平面上两个向量的叉乘。

法向量 * 距离形式

N为法向量(1x3的向量)，那么[N, d]或者(N | d)为 1x4 的向量，其中d为平面到原点的距离。

旋转问题

现在平面的表示形式(无论哪种)都是基于坐标系A，另有一个坐标系B，从B到A的变换矩阵是M(4x4)，那么平面在坐标系B下的表示形式是什么？

向量 ，注意不是法向量，平面上一点为 ，那么 ，经过变换后，点v变成了 ，那么有 ，也就有，因此可得 ，M矩阵的逆根据《SLAM十四讲》的公式3.13可以直接写出，最后根据 写成一般式

感觉这样推导并不严谨，但可以这样理解。

参考：OpenGL Normal Vector Transformation