沉默杀手HomeArchivesTagsCategories
cartographer 常用的类型
|激光SLAMCartographer源码解读|
Word count: 1.5k|Reading time: 8 min

Rigid3类型在rigid_transform.h，很有学习价值

1
2
3
4
5
Vector translation_;
Quaternion rotation_;
// using Vector = Eigen::Matrix<FloatType, 3, 1>;
// using Quaternion = Eigen::Quaternion<FloatType>;
// using AngleAxis = Eigen::AngleAxis<FloatType>;

Rigid3d类型实质是包含了一个3行1列的Eigen矩阵，一个Eigen::Quaternion，可以直接用 cout
值得注意的运算符：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// 参见 SLAM 十四讲
Rigid3 inverse() const
{
  const Quaternion rotation = rotation_.conjugate();
  const Vector translation = -(rotation * translation_);
  return Rigid3(translation, rotation);
}

template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                            const Rigid3<FloatType>& rhs) {
  return Rigid3<FloatType>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      (lhs.rotation() * rhs.rotation()).normalized());
}

OptimizationProblem2D类的成员

struct NodeId，成员只有int trajectory_id; 和 int node_index;，后者是从0开始的

struct SubmapId，成员只有int trajectory_id; 和 int submap_index;，后者是从0开始的

class MapById是对std::map的一个封装，其实是模板<typename IdType, typename DataType>，前者可以是NodeId 或者 SubmapId

// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
struct NodeSpec2D
{
  common::Time time;
  transform::Rigid2d local_pose_2d;
  transform::Rigid2d global_pose_2d;
  Eigen::Quaterniond gravity_alignment;
};

struct SubmapSpec2D
{
  transform::Rigid2d global_pose;
};
// optimization_problem_->submap_data()  就是 SubmapSpec2D
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;

data_是类PoseGraph2D中的成员变量，类型 PoseGraphData

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
struct PoseGraphData
{
  /* struct InternalSubmapData
  {
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;

  // IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
    std::set<NodeId>  node_ids;
  }; */

  // Submaps get assigned an ID and state as soon as they are seen, even
  // before they take part in the background computations.
  MapById<SubmapId, InternalSubmapData> submap_data;

  // Global submap poses currently used for displaying data.
  MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
  MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;

  // Data that are currently being shown.
  MapById<NodeId, TrajectoryNode> trajectory_nodes;

  // Global landmark poses with all observations.
  std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
      landmark_nodes;
  // How our various trajectories are related.
  TrajectoryConnectivityState trajectory_connectivity_state;
  int num_trajectory_nodes = 0;
  std::map<int, InternalTrajectoryState> trajectories_state;

  // Set of all initial trajectory poses.
  std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;

  std::vector<PoseGraphInterface::Constraint> constraints;
};

1
2
3
4
5
6
7
8
9
struct InternalSubmapData
{
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;
  // IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
  std::set<NodeId> node_ids;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
struct Constraint {
  struct Pose {
    transform::Rigid3d zbar_ij;
    double translation_weight;
    double rotation_weight;
  };
  SubmapId submap_id;  // 'i' in the paper.
  NodeId node_id;      // 'j' in the paper.
  // Pose of the node 'j' relative to submap 'i'.
  Pose pose;
/*  Differentiates between intra-submap (where node 'j' was inserted into
  submap 'i') and inter-submap constraints (where node 'j' was not inserted
  into submap 'i').*/
  enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
class OptimizationProblem2D
    : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
                                          transform::Rigid2d>
{
public:
    void Solve(
      const std::vector<Constraint>& constraints,
      const std::map<int, PoseGraphInterface::TrajectoryState>&
          trajectories_state,
      const std::map<std::string, LandmarkNode>& landmark_nodes) override;
    // 省略很多函数
private:
  optimization::proto::OptimizationProblemOptions  options_;
  MapById<NodeId, NodeSpec2D>  node_data_;
  MapById<SubmapId, SubmapSpec2D> submap_data_;
  sensor::MapByTime<sensor::ImuData> imu_data_;
  sensor::MapByTime<sensor::OdometryData> odometry_data_;
  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;

  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
  std::map<std::string, transform::Rigid3d> landmark_data_;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
// 结果、时间、函数
struct WorkItem
{
  enum class Result {
    kDoNotRunOptimization,
    kRunOptimization,
  };

  std::chrono::steady_clock::time_point time;
  std::function<Result()> task;
};

using WorkQueue = std::deque<WorkItem>;

ConstraintBuilder2D::Result 其实是 vector<Constraint>

PoseExtrapolator

1
2
3
std::deque<TimedPose> timed_pose_queue_;
std::deque<sensor::ImuData> imu_data_;
std::deque<sensor::OdometryData> odometry_data_;
cartographer里凡是变量名包含queue的，类型都是deque

插入子图

sensor::RangeData

local坐标系下的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
// Rays begin at 'origin'. 'returns' are the points where obstructions were
// detected. 'misses' are points in the direction of rays for which no return
// was detected, and were inserted at a configured distance
// It is assumed that between the 'origin' and 'misses' is free space
struct RangeData {
  Eigen::Vector3f  origin;
  PointCloud  returns;
  PointCloud  misses;
};

using PointCloud = std::vector<RangefinderPoint>;
// Stores 3D position of a point observed by a rangefinder sensor.
struct RangefinderPoint
{
	Eigen::Vector3f position;
};

TimedRangeData

1
2
3
4
5
struct TimedPointCloudData {
  common::Time time;
  Eigen::Vector3f origin;
  TimedPointCloud ranges;
};
1
2
3
4
5
6
7
8
9
struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;
    size_t origin_index;
  };
  common::Time time;
  std::vector<Eigen::Vector3f> origins;
  std::vector<RangeMeasurement> ranges;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
// Like 'RangeData', but with 'TimedPointClouds'.
struct TimedRangeData {
  Eigen::Vector3f origin;
  TimedPointCloud returns;
  TimedPointCloud misses;
};

using TimedPointCloud = std::vector<TimedRangefinderPoint>;

struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time;
};
1
2
3
4
5
6
7
8
9
10
11
struct SubmapPose
{
	int version;
	transform::Rigid3d  pose;
};

struct SubmapData
{
	std::shared_ptr<const Submap> submap;
	transform::Rigid3d  pose;
};

LocalTrajectoryBuilder2D

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
struct MatchingResult
{
    common::Time time;    // 扫描匹配发生的时间
    // tracking 坐标系下的位姿(Local SLAM坐标系下的位姿), 需要乘以
    // tracking frame in map的转换(Local SLAM坐标系到Global SLAM坐标系的转换)才能转到全局坐标系
    transform::Rigid3d local_pose;
    // tracking 坐标系下的点云（即LocalSLAM坐标系下的点云）
    sensor::RangeData range_data_in_local;
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
};

struct InsertionResult
{
    // Node数据   插入的节点数据，TrajectoryNode::Data包含了处理之后的点云数据
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    //该Node数据插入的两个submap
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};

struct TrajectoryNode
{
  struct Data
  {
    common::Time  time;
    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond  gravity_alignment;
    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud  filtered_gravity_aligned_point_cloud;
    // 省略 3D.

    // The node pose in the local SLAM frame.
    transform::Rigid3d   local_pose;
  };
  common::Time time() const { return constant_data->time; }
  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data;
  // The node pose in the global SLAM frame.
  transform::Rigid3d  global_pose;
};

MatchingResult里有一个local_pose，InsertionResult的constant_data成员里也有一个local_pose， 这两个是相同的，赋值用的都是下一帧位姿的观测值。不过在后端里用的是InsertionResult的local_pose，源码在PoseGraph2D::AddNode

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// An individual submap, which has a 'local_pose' in the local map frame, 
// keeps track of how many range data were inserted into it
// and sets 'insertion_finished' when the map no longer changes 
// and is ready for loop closing
class Submap
{
public:
	Submap(const transform::Rigid3d& local_submap_pose)
	  : local_pose_(local_submap_pose) {}

private:
	// Pose of this submap in the local map frame
	const transform::Rigid3d  local_pose_;
	// Number of RangeData inserted.
	int num_range_data_ = 0;
	bool insertion_finished_ = false;
};

常用函数

1
2
3
4
5
6
7
8
9
10
11
12
// 对点云中每一个点做变换， 左乘 transform
PointCloud TransformPointCloud(const PointCloud&  point_cloud,
                       const transform::Rigid3f&  transform)
{
	PointCloud result;
	result.reserve(point_cloud.size()  );
	for (const RangefinderPoint& point : point_cloud)
	{
		result.emplace_back(transform * point);
	}
	return result;
}
1
2
3
4
5
6
7
8
9
RangeData TransformRangeData(const RangeData& range_data,
                             const transform::Rigid3f& transform)
{
  return RangeData{
      transform * range_data.origin,
      TransformPointCloud(range_data.returns, transform),
      TransformPointCloud(range_data.misses, transform),
  };
}

Trajectory

enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };