1. The code goes to
1.1 nodemain.cc
// 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
1.2 node.cc
// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
// 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
CHECK(ValidateTrajectoryOptions(options));
// 添加一条轨迹
AddTrajectory(options);
}
1.3 node.cc
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
// 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
// 新增一个位姿估计器
AddExtrapolator(trajectory_id, options);
// 新生成一个传感器数据采样器
AddSensorSamplers(trajectory_id, options);
// 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);
// 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
// 检查设置的topic名字是否在ros中存在, 不存在则报错
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
// 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
1.4 node.cc
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id) {
// laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});
}
// multi_echo_laser_scans的订阅与注册回调函数
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this),
topic});
}
// point_clouds 的订阅与注册回调函数
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::PointCloud2>(
&Node::HandlePointCloud2Message, trajectory_id, topic,
&node_handle_, this),
topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
// imu 的订阅与注册回调函数,只有一个imu的topic
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, kImuTopic,
&node_handle_, this),
kImuTopic});
}
// odometry 的订阅与注册回调函数,只有一个odometry的topic
if (options.use_odometry) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, kOdometryTopic,
&node_handle_, this),
kOdometryTopic});
}
// gps 的订阅与注册回调函数,只有一个gps的topic
if (options.use_nav_sat) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::NavSatFix>(
&Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
&node_handle_, this),
kNavSatFixTopic});
}
// landmarks 的订阅与注册回调函数,只有一个landmarks的topic
if (options.use_landmarks) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
&Node::HandleLandmarkMessage,
trajectory_id,
kLandmarkTopic,
&node_handle_,
this),
kLandmarkTopic});
}
}
// 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
return options.trajectory_builder_options
.has_trajectory_builder_2d_options();
}
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
return options.trajectory_builder_options
.has_trajectory_builder_3d_options();
}
return false;
}
// 检查topic名字是否被其他轨迹使用
bool Node::ValidateTopicNames(const TrajectoryOptions& options) {
for (const auto& sensor_id : ComputeExpectedSensorIds(options)) {
const std::string& topic = sensor_id.id;
// 如果topic能够在subscribed_topics_中找到, 证明这个topic名字被之前的轨迹使用了
if (subscribed_topics_.count(topic) > 0) {
LOG(ERROR) << "Topic name [" << topic << "] is already used.";
return false;
}
}
return true;
}
1.5 node.cc
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 根据配置,是否将传感器数据跳过
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
1.6 node.cc
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 根据配置,是否将传感器数据跳过
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
1.7sensor_bridge.cc
// 处理LaserScan数据, 先转成点云,再传入trajectory_builder_
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
1.8sensor_bridge.cc
// 根据参数配置,将一帧雷达数据分成几段, 再传入trajectory_builder_
void SensorBridge::HandleLaserScan(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id,
const carto::sensor::PointCloudWithIntensities& points) {
if (points.points.empty()) {
return;
}
// CHECK_LE: 小于等于
CHECK_LE(points.points.back().time, 0.f);
// TODO(gaschler): Use per-point time instead of subdivisions.
// 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
// 生成分段的点云
carto::sensor::TimedPointCloud subdivision(
points.points.begin() + start_index, points.points.begin() + end_index);
if (start_index == end_index) {
continue;
}
const double time_to_subdivision_end = subdivision.back().time;
// `subdivision_time` is the end of the measurement so sensor::Collator will
// send all other sensor data first.
const carto::common::Time subdivision_time =
time + carto::common::FromSeconds(time_to_subdivision_end);
auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
if (it != sensor_to_previous_subdivision_time_.end() &&
// 上一段点云的时间不应该大于等于这一段点云的时间
it->second >= subdivision_time) {
LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
<< sensor_id << " because previous subdivision time "
<< it->second << " is not before current subdivision time "
<< subdivision_time;
continue;
}
// 更新对应sensor_id的时间戳
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
// 检查点云的时间
for (auto& point : subdivision) {
point.time -= time_to_subdivision_end;
}
CHECK_EQ(subdivision.back().time, 0.f);
// 将分段后的点云 subdivision 传入 trajectory_builder_
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
} // for
}
1.9sensor_bridge.cc
// 雷达相关的数据最终的处理函数
// 先将数据转到tracking坐标系下,再调用trajectory_builder_的AddSensorData进行数据的处理
/**
* @brief
*
* @param[in] sensor_id 数据的话题
* @param[in] time 点云的时间戳(最后一个点的时间)
* @param[in] frame_id 点云的frame
* @param[in] ranges 雷达坐标系下的TimedPointCloud格式的点云
*/
void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
if (!ranges.empty()) {
CHECK_LE(ranges.back().time, 0.f);
}
const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
// 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin
// 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time,
sensor_to_tracking->translation().cast<float>(),
// 将点云从雷达坐标系下转到tracking_frame坐标系系下
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())} ); // 强度始终为空
}
}
1.10global_trajectory_builder.cc
* @brief 点云数据的处理, 先进行扫描匹配, 然后将扫描匹配的结果当做节点插入到后端的位姿图中
*
* @param[in] sensor_id topic名字
* @param[in] timed_point_cloud_data 点云数据
*/
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
CHECK(local_trajectory_builder_)
<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
// 进行扫描匹配, 返回匹配后的结果
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
sensor_id, timed_point_cloud_data);
if (matching_result == nullptr) {
// The range data has not been fully accumulated yet.
return;
}
kLocalSlamMatchingResults->Increment();
std::unique_ptr<InsertionResult> insertion_result;
// matching_result->insertion_result 的类型是 LocalTrajectoryBuilder2D::InsertionResult
// 如果雷达成功插入到地图中
if (matching_result->insertion_result != nullptr) {
kLocalSlamInsertionResults->Increment();
// 将匹配后的结果 当做节点 加入到位姿图中
auto node_id = pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps);
CHECK_EQ(node_id.trajectory_id, trajectory_id_);
// 这里的InsertionResult的类型是 TrajectoryBuilderInterface::InsertionResult
insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
node_id,
matching_result->insertion_result->constant_data,
std::vector<std::shared_ptr<const Submap>>(
matching_result->insertion_result->insertion_submaps.begin(),
matching_result->insertion_result->insertion_submaps.end())});
}
// 将结果数据传入回调函数中, 进行保存
if (local_slam_result_callback_) {
local_slam_result_callback_(
trajectory_id_, matching_result->time, matching_result->local_pose,
std::move(matching_result->range_data_in_local),
std::move(insertion_result));
}
}
1.11local_trajectory_builder_2d.cc
/**
* @brief 处理点云数据, 进行扫描匹配, 将点云写成地图
*
* @param[in] sensor_id 点云数据对应的话题名称
* @param[in] unsynchronized_data 传入的点云数据
* @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> 匹配后的结果
*/
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data) {
// Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的
auto synchronized_data =
range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
if (synchronized_data.ranges.empty()) {
LOG(INFO) << "Range data collator filling buffer.";
return nullptr;
}
const common::Time& time = synchronized_data.time;
// Initialize extrapolator now if we do not ever use an IMU.
// 如果不用imu, 就在雷达这初始化位姿推测器
if (!options_.use_imu_data()) {
InitializeExtrapolator(time);
}
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator with our first IMU message, we
// cannot compute the orientation of the rangefinder.
LOG(INFO) << "Extrapolator not yet initialized.";
return nullptr;
}
CHECK(!synchronized_data.ranges.empty());
// TODO(gaschler): Check if this can strictly be 0.
CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
// 计算第一个点的时间
const common::Time time_first_point =
time +
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
// 只有在extrapolator_初始化时, GetLastPoseTime()是common::Time::min()
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
std::vector<transform::Rigid3f> range_data_poses;
range_data_poses.reserve(synchronized_data.ranges.size());
bool warned = false;
// 预测得到每一个时间点的位姿
for (const auto& range : synchronized_data.ranges) {
common::Time time_point = time + common::FromSeconds(range.point_time.time);
// 如果该时间比上次预测位姿的时间还要早,说明这个点的时间戳往回走了, 就报错
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
// 一个循环只报一次错
if (!warned) {
LOG(ERROR)
<< "Timestamp of individual range data point jumps backwards from "
<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
warned = true;
}
time_point = extrapolator_->GetLastExtrapolatedTime();
}
// Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
}
if (num_accumulated_ == 0) {
// 'accumulated_range_data_.origin' is uninitialized until the last
// accumulation.
accumulated_range_data_ = sensor::RangeData{
{}, {}, {}};
}
// Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses.
// 对每个数据点进行处理
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
// 获取在tracking frame 下点的坐标
const sensor::TimedRangefinderPoint& hit =
synchronized_data.ranges[i].point_time;
// 将点云的origins坐标转到 local slam 坐标系下
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
// Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
sensor::RangefinderPoint hit_in_local =
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
// 计算这个点的距离, 这里用的是去畸变之后的点的距离
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();
// param: min_range max_range
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
// 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标
accumulated_range_data_.returns.push_back(hit_in_local);
} else {
// Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里
hit_in_local.position =
origin_in_local +
// param: missing_data_ray_length, 是个比例, 不是距离
options_.missing_data_ray_length() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
}
}
} // end for
// 有一帧有效的数据了
++num_accumulated_;
// param: num_accumulated_range_data 几帧有效的点云数据进行一次扫描匹配
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
// 计算2次有效点云数据的的时间差
const common::Time current_sensor_time = synchronized_data.time;
absl::optional<common::Duration> sensor_duration;
if (last_sensor_time_.has_value()) {
sensor_duration = current_sensor_time - last_sensor_time_.value();
}
last_sensor_time_ = current_sensor_time;
// 重置变量
num_accumulated_ = 0;
// 获取机器人当前姿态
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time));
// TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
// 'time'.
// 以最后一个点的时间戳估计出的坐标为这帧数据的原点
accumulated_range_data_.origin = range_data_poses.back().translation();
return AddAccumulatedRangeData(
time,
// 将点云变换到local原点处, 且姿态为0
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment, sensor_duration);
}
return nullptr;
}
1.12local_trajectory_builder_2d.cc
/**
* @brief 进行扫描匹配, 将点云写入地图
*
* @param[in] time 点云的时间戳
* @param[in] gravity_aligned_range_data 原点位于local坐标系原点处的点云
* @param[in] gravity_alignment 机器人当前姿态
* @param[in] sensor_duration 2帧点云数据的时间差
* @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
*/
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment,
const absl::optional<common::Duration>& sensor_duration) {
// 如果处理完点云之后数据为空, 就报错. 使用单线雷达时不要设置min_z
if (gravity_aligned_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty horizontal range data.";
return nullptr;
}
// Computes a gravity aligned pose prediction.
// 进行位姿的预测, 先验位姿
const transform::Rigid3d non_gravity_aligned_pose_prediction =
extrapolator_->ExtrapolatePose(time);
// 将三维位姿先旋转到姿态为0, 再取xy坐标将三维位姿转成二维位姿
const transform::Rigid2d pose_prediction = transform::Project2D(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
// Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloud
const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
options_.adaptive_voxel_filter_options());
if (filtered_gravity_aligned_point_cloud.empty()) {
return nullptr;
}
// local map frame <- gravity-aligned frame
// 扫描匹配, 进行点云与submap的匹配
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr) {
LOG(WARNING) << "Scan matching failed.";
return nullptr;
}
// 将二维坐标旋转回之前的姿态
const transform::Rigid3d pose_estimate =
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
// 校准位姿估计器
extrapolator_->AddPose(time, pose_estimate);
// Step: 8 将 原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云
sensor::RangeData range_data_in_local =
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d->cast<float>()));
// 将校正后的雷达数据写入submap
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
// 计算耗时
const auto wall_time = std::chrono::steady_clock::now();
if (last_wall_time_.has_value()) {
const auto wall_time_duration = wall_time - last_wall_time_.value();
kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
if (sensor_duration.has_value()) {
kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /
common::ToSeconds(wall_time_duration));
}
}
// 计算cpu耗时
const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
if (last_thread_cpu_time_seconds_.has_value()) {
const double thread_cpu_duration_seconds =
thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
if (sensor_duration.has_value()) {
kLocalSlamCpuRealTimeRatio->Set(
common::ToSeconds(sensor_duration.value()) /
thread_cpu_duration_seconds);
}
}
last_wall_time_ = wall_time;
last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
// 返回结果
return absl::make_unique<MatchingResult>(
MatchingResult{time, pose_estimate, std::move(range_data_in_local),
std::move(insertion_result)});
}
1.13 local_trajectory_builder_2d.cc
/**
* @brief 进行扫描匹配
*
* @param[in] time 点云的时间
* @param[in] pose_prediction 先验位姿
* @param[in] filtered_gravity_aligned_point_cloud 匹配用的点云
* @return std::unique_ptr<transform::Rigid2d> 匹配后的二维位姿
*/
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
if (active_submaps_.submaps().empty()) {
return absl::make_unique<transform::Rigid2d>(pose_prediction);
}
// 使用active_submaps_的第一个子图进行匹配
std::shared_ptr<const Submap2D> matching_submap =
active_submaps_.submaps().front();
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
transform::Rigid2d initial_ceres_pose = pose_prediction;
// 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
if (options_.use_online_correlative_scan_matching()) {
const double score = real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), &initial_ceres_pose);
kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
}
auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
// 使用ceres进行扫描匹配
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), pose_observation.get(),
&summary);
// 一些度量
if (pose_observation) {
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
const double residual_distance =
(pose_observation->translation() - pose_prediction.translation())
.norm();
kScanMatcherResidualDistanceMetric->Observe(residual_distance);
const double residual_angle =
std::abs(pose_observation->rotation().angle() -
pose_prediction.rotation().angle());
kScanMatcherResidualAngleMetric->Observe(residual_angle);
}
// 返回ceres计算后的位姿
return pose_observation;
}
1.14local_trajectory_builder_2d.cc
2. Scanning matching understanding
2.1 Scanning matching includes two algorithms, correlation scanning matching and ceres scanning matching; correlation scanning matching can be enabled or disabled, which is equivalent to a fine calibration of poses using brute force search, which consumes a lot of resources; Correlation scan matching is based on the grid, but ceres is not, the grid resolution accuracy is 0.05cm, and the accuracy of ceres is higher
2.2 The input is: the prior pose provided by the pose estimator and the preprocessed point cloud output is: pose
Purpose
1. Calibrate the pose estimator
// 校准位姿估计器
extrapolator_->AddPose(time, pose_estimate);
2. Rotate the point cloud in the local coordinate system to the coordinate system after the pose is obtained
// Step: 8 将 原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云
sensor::RangeData range_data_in_local =
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d->cast<float>()));
3. Write the calibrated point cloud to submap
// 将校正后的雷达数据写入submap
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());