一、前言
在之前的文章中，主要对Cartographer前端定位部分的接口进行了梳理，在这一部分将对其主要作用的，完成帧间匹配、地图构建与插入的LocalTrajectoryBuilder部分进行分析，以LocalTrajectoryBuilder2D为例，3D与之类似，主体结构相同。
二、LocalTrajectoryBuilder2D分析
除了上篇提到的几个函数之外，细心观察整个LocalTrajectoryBuilder2D.cc文件，可以发现剩下的几个函数基本都在AddRangeData这个函数中被调用了，而且它本身也是占据了这个.cc文件一半内容的巨大函数，实现了帧间匹配、地图构建与插入的关键功能
//返回类型是MatchingResult，上.h文件里找了一下，主要有三个内容
//扫描时间、在局部地图下的位姿、在局部地图下的传感器数据
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
//传感器的索引
const std::string& sensor_id,
//未校正的传感器数据
const sensor::TimedPointCloudData& unsynchronized_data) {
//矫正后的传感器数据
auto synchronized_data =
//这里调用的是range_data_collator.cc中的AddRangeData函数
//主要实现的功能室对队列中可能存在的同一传感器多组数据的处理，做时间同步，排序返回
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);
}
//如果使用IMU数据，必须等到接收到IMU的数据后才开始
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;
}
//确保有数据，且最后一个数据的时间不是0
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_的构建
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) {
//校正后的time + 传感器时间戳
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();
}
range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
}
//如果历史记录中没有数据，就用num_accumulated_来记录
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.
//舍弃所有range信息不合适的传感器数据
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
const sensor::TimedRangefinderPoint& hit =
synchronized_data.ranges[i].point_time;
//将机器人坐标系转化到局部地图坐标系
//将synchronized_data中的位姿左乘range_data_poses中的机器人位姿
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
//有效点，左乘range_data_poses中的机器人位姿
sensor::RangefinderPoint hit_in_local =
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
//转化后传感器坐标系和hit坐标系的差值
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();
//舍弃距离不合适的传感器信息
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit_in_local);
} else {
hit_in_local.position =
origin_in_local +
options_.missing_data_ray_length() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
}
}
}
++num_accumulated_;
//如果计数器累计的数目大于num_accumulated_range_data，进行地图插入
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
//一些时间处理
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,
//以g值方向为基准，修正传感器位姿，体素滤波
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment, sensor_duration);
}
return nullptr;
}可以十分明显的发现，cartographer在不调用来调用去的时候，可读性还是不错的。AddRangeData这个大函数主要完成的功能是接受到传感器数据后，进行时间同步处理，然后将传感器数据转换到子图坐标系下进行处理，在堆叠够一定量的数据后，形成子图
传感器校正的时候调用的是range_data_collator.cc中的AddRangeData函数
//local_trajectory_builder_2d.cc 中 std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>LocalTrajectoryBuilder2D::AddRangeData调用
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
const std::string& sensor_id,
sensor::TimedPointCloudData timed_point_cloud_data) {
//检测该数据sensor_id是否在期望的sensor_ids里面，否则程序停止
CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
timed_point_cloud_data.intensities.resize(
timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
// TODO(gaschler): These two cases can probably be one.
//当sensor_id不在id_to_pending_data_中时
if (id_to_pending_data_.count(sensor_id) != 0) {
//上一帧的结束位（时间）
current_start_ = current_end_;
// When we have two messages of the same sensor, move forward the older of
// the two (do not send out current).
current_end_ = id_to_pending_data_.at(sensor_id).time;
auto result = CropAndMerge();
//把新的点云插入进去了
id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
return result;
}
//当sensor_id在id_to_pending_data_中时
id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
//规定的一帧的长度 != 输入的点云信息的长度(不完整？)
if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
return {};
}
current_start_ = current_end_;
// We have messages from all sensors, move forward to oldest.
common::Time oldest_timestamp = common::Time::max();
for (const auto& pair : id_to_pending_data_) {
oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
}
current_end_ = oldest_timestamp;
return CropAndMerge();
}
sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
sensor::TimedPointCloudOriginData result{current_end_, {}, {}};
bool warned_for_dropped_points = false;
//遍历id_to_pending_data_中所有传感器的数据，其对于传感器数据处理一样
for (auto it = id_to_pending_data_.begin();
it != id_to_pending_data_.end();) {
sensor::TimedPointCloudData& data = it->second;
const sensor::TimedPointCloud& ranges = it->second.ranges;
const std::vector<float>& intensities = it->second.intensities;
//数据的起始点
auto overlap_begin = ranges.begin();
while (overlap_begin < ranges.end() &&
data.time + common::FromSeconds((*overlap_begin).time) <
current_start_) {
++overlap_begin;
}
//数据的结束点
auto overlap_end = overlap_begin;
while (overlap_end < ranges.end() &&
data.time + common::FromSeconds((*overlap_end).time) <=
current_end_) {
++overlap_end;
}
//把overlap_begin之前的数据去掉了
if (ranges.begin() < overlap_begin && !warned_for_dropped_points) {
LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin)
<< " earlier points.";
warned_for_dropped_points = true;
}
// Copy overlapping range.
//复制重叠范围
if (overlap_begin < overlap_end) {
std::size_t origin_index = result.origins.size();
result.origins.push_back(data.origin);
const float time_correction =
static_cast<float>(common::ToSeconds(data.time - current_end_));
auto intensities_overlap_it =
intensities.begin() + (overlap_begin - ranges.begin());
result.ranges.reserve(result.ranges.size() +
std::distance(overlap_begin, overlap_end));
for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
++overlap_it, ++intensities_overlap_it) {
sensor::TimedPointCloudOriginData::RangeMeasurement point{
*overlap_it, *intensities_overlap_it, origin_index};
// current_end_ + point_time[3]_after == in_timestamp +
// point_time[3]_before
point.point_time.time += time_correction;
result.ranges.push_back(point);
}
}
// Drop buffered points until overlap_end.
//将缓冲点丢弃，直到overlap_end
if (overlap_end == ranges.end()) {
it = id_to_pending_data_.erase(it);
} else if (overlap_end == ranges.begin()) {
++it;
} else {
const auto intensities_overlap_end =
intensities.begin() + (overlap_end - ranges.begin());
data = sensor::TimedPointCloudData{
data.time, data.origin,
sensor::TimedPointCloud(overlap_end, ranges.end()),
std::vector<float>(intensities_overlap_end, intensities.end())};
++it;
}
}
//按照时间排序，并且返回结果
std::sort(result.ranges.begin(), result.ranges.end(),
[](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
return a.point_time.time < b.point_time.time;
});
return result;
}这个函数比较注重有关时间戳的处理。对多组数据同时存在时，主要用了overlap_begin和overlap_end两个左右指针作滑窗，每次新的处理开始时，overlap_begin滑到overlap_end，overlap_end滑到传进来数据的末尾点
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const {
const sensor::RangeData cropped =
sensor::CropRangeData(sensor::TransformRangeData(
range_data, transform_to_gravity_aligned_frame),
options_.min_z(), options_.max_z());
return sensor::RangeData{
cropped.origin,
sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),
sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}
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) {
if (gravity_aligned_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty horizontal range data.";
return nullptr;
}
//都统一到重力方向，计算位姿变化
// Computes a gravity aligned pose prediction.
//没有重力修正时候的位姿，直接从extrapolator_里读
const transform::Rigid3d non_gravity_aligned_pose_prediction =
extrapolator_->ExtrapolatePose(time);
//无修正位姿右乘重力矩阵的逆
const transform::Rigid2d pose_prediction = transform::Project2D(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
//下采样，但是这里下采样的是重力修正过的传感器数据,是上一次优化的结果
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
//扫描匹配，调用ScanMatch函数，计算pose_estimate_2d
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;
}
//右乘重力矩阵，转回原来的坐标系，插入extrapolator_
const transform::Rigid3d pose_estimate =
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
extrapolator_->AddPose(time, pose_estimate);
//将传感器数据转化到新的坐标系下，插入子图
sensor::RangeData range_data_in_local =
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d->cast<float>()));
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
//记录耗时，装到wall_time_duration
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)});
}
地图插入的时候调用AddAccumulatedRangeData函数，函数输入量用到了TransformToGravityAlignedFrameAndFilter，用来将传感器信息修正在重力向量的方向上，在函数中，扫描匹配的时候用了ScanMatch函数
在scan-map计算位姿变化的时候，首先将上一帧的点云信息提出来，同时将这一帧的也提出来，都根据重力方向修正之后，进行匹配，调用ScanMatch函数，而后又将点云转回去，消除重力方向，调用InsertIntoSubmap插入子图。
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;
//这里调用的是real_time_correlative_scan_matcher_2d.cc的Match函数
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);
}
//建立最小二乘问题，调用ceres，得到最大概率位姿估计
auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
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);
}
return pose_observation;
}
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_local,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
const transform::Rigid3d& pose_estimate,
const Eigen::Quaterniond& gravity_alignment) {
//运动过滤器判断一下，要是运动量不大就直接返回了
if (motion_filter_.IsSimilar(time, pose_estimate)) {
return nullptr;
}
//点云数据暂存到地图指针里
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
active_submaps_.InsertRangeData(range_data_in_local);
//InsertionResult里有两部分：插入节点数据、插入的子图
return absl::make_unique<InsertionResult>(InsertionResult{
std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
time,
gravity_alignment,
filtered_gravity_aligned_point_cloud,
{}, // 'high_resolution_point_cloud' is only used in 3D.
{}, // 'low_resolution_point_cloud' is only used in 3D.
{}, // 'rotational_scan_matcher_histogram' is only used in 3D.
pose_estimate}),
std::move(insertion_submaps)});
}ScanMatch函数也比较麻烦，将输入位姿利用ceres转化为了一个最小二乘问题，调用了real_time_correlative_scan_matcher_2d.cc的Match函数，计算出了最优匹配结果，然后将匹配结果返回
double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const Grid2D& grid,
transform::Rigid2d* pose_estimate) const {
CHECK(pose_estimate != nullptr);
//初始位姿、子图点云（旋转至初始位姿下）、搜索空间（线速度搜索窗口、角速度搜索窗口）
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(),
rotated_point_cloud, grid.limits().resolution());
//先对yaw角搜索，再对xy坐标搜索，有效减少计算量
//调用correlative_scan_matcher_2d.cc下的GenerateRotatedScans函数
//获得搜索窗口下机器人朝向各个方向角时的点云数据
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
//点云离散化处理 这里没看懂
//浮点类型的点云数据转换成整型的栅格单元索引,将旋转点云，从旋转点云坐标中，转化到地图坐标中去
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
//生成所有候选位置，并打分
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
//分数最高的作为预测位姿,xy各自加上预测，yaw角直接取预测值
const Candidate2D& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return best_candidate.score;
}Match函数的主要思路是，把输入的点云按照一个角分辨率进行不停地旋转，调用了correlative_scan_matcher_2d.cc下的GenerateRotatedScans函数（又开始套娃了），得到一组旋转了不同角度的点云组，然后再对xy进行暴力匹配
std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters) {
std::vector<sensor::PointCloud> rotated_scans;
rotated_scans.reserve(search_parameters.num_scans);
//角分辨率
double delta_theta = -search_parameters.num_angular_perturbations *
search_parameters.angular_perturbation_step_size;
for (int scan_index = 0; scan_index < search_parameters.num_scans;
++scan_index,
delta_theta += search_parameters.angular_perturbation_step_size) {
//每次scan_index增加，相当于point_cloud多转过一个角度
rotated_scans.push_back(sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));
}
return rotated_scans;
}
std::vector<DiscreteScan2D> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan2D> discrete_scans;
discrete_scans.reserve(scans.size());
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size());
for (const sensor::RangefinderPoint& point : scan) {
//从旋转点云坐标中，转化到地图坐标中去
const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.position.head<2>();
//获取对应的栅格索引点
discrete_scans.back().push_back(
map_limits.GetCellIndex(translated_point));
}
}
return discrete_scans;
}
std::vector<Candidate2D>
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
const SearchParameters& search_parameters) const {
int num_candidates = 0;
//x和y方向的滑窗大小
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
const int num_linear_x_candidates =
(search_parameters.linear_bounds[scan_index].max_x -
search_parameters.linear_bounds[scan_index].min_x + 1);
const int num_linear_y_candidates =
(search_parameters.linear_bounds[scan_index].max_y -
search_parameters.linear_bounds[scan_index].min_y + 1);
num_candidates += num_linear_x_candidates * num_linear_y_candidates;
}
//累增xy，升成对应格子上的candidates
std::vector<Candidate2D> candidates;
candidates.reserve(num_candidates);
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
++x_index_offset) {
for (int y_index_offset =
search_parameters.linear_bounds[scan_index].min_y;
y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
++y_index_offset) {
candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
search_parameters);
}
}
}
CHECK_EQ(candidates.size(), num_candidates);
return candidates;
}
建立最小二乘问题后，最后一部分内容是打分，这部分实在是看不太懂
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates) const {
for (Candidate2D& candidate : *candidates) {
switch (grid.GetGridType()) {
//如果是概率图
case GridType::PROBABILITY_GRID:
candidate.score = ComputeCandidateScore(
static_cast<const ProbabilityGrid&>(grid),
discrete_scans[candidate.scan_index], candidate.x_index_offset,
candidate.y_index_offset);
break;
//如果是TSDF地图
case GridType::TSDF:
candidate.score = ComputeCandidateScore(
static_cast<const TSDF2D&>(grid),
discrete_scans[candidate.scan_index], candidate.x_index_offset,
candidate.y_index_offset);
break;
}
candidate.score *=
std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *
options_.translation_delta_cost_weight() +
std::abs(candidate.orientation) *
options_.rotation_delta_cost_weight()));
}
}
float ComputeCandidateScore(const TSDF2D& tsdf,
const DiscreteScan2D& discrete_scan,
int x_index_offset, int y_index_offset) {
float candidate_score = 0.f;
float summed_weight = 0.f;
for (const Eigen::Array2i& xy_index : discrete_scan) {
const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
xy_index.y() + y_index_offset);
const std::pair<float, float> tsd_and_weight =
tsdf.GetTSDAndWeight(proposed_xy_index);
const float normalized_tsd_score =
(tsdf.GetMaxCorrespondenceCost() - std::abs(tsd_and_weight.first)) /
tsdf.GetMaxCorrespondenceCost();
const float weight = tsd_and_weight.second;
candidate_score += normalized_tsd_score * weight;
summed_weight += weight;
}
if (summed_weight == 0.f) return 0.f;
candidate_score /= summed_weight;
CHECK_GE(candidate_score, 0.f);
return candidate_score;
}
float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
const DiscreteScan2D& discrete_scan,
int x_index_offset, int y_index_offset) {
float candidate_score = 0.f;
for (const Eigen::Array2i& xy_index : discrete_scan) {
const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
xy_index.y() + y_index_offset);
const float probability =
probability_grid.GetProbability(proposed_xy_index);
candidate_score += probability;
}
candidate_score /= static_cast<float>(discrete_scan.size());
CHECK_GT(candidate_score, 0.f);
return candidate_score;
}默认用的是概率图，tsd我也没接触过，看不太懂额额，感觉涉及到概率地图里面的好多东西了，下一篇文章里再做细致的分析吧
当返回打分后，根据最佳分数获得估计的位姿，整个Match就结束了，同时也完成了向子地图的插入，整个LocalTrajectoryBuilder的内容就结束了
三、后话
LocalTrajectoryBuilder部分关于帧间匹配、地图构建与插入的代码比之前接口类的代码读起来要简单一点，关于地图的构建，匹配的时候估计位姿的打分确实分析的不太好，打算下一篇文章根据这些内容再展开看一下
7875
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
