map_builder.h
是算法的入口，封装了local submap和 global pose-graph的优化
int MapBuilder:: AddTrajectoryBuiler() {
std::unique_ptr local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder = absl::make_unique(
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids));
}
DCHECK(dynamic_cast(pose_graph_.get()));
// 创建global trajectory的builder
trajectory_builders_.push_back(absl::make_unique(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast(pose_graph_.get()),
local_slam_result_callback)));
}
// 通过这个去处理range data， imu的数据
mapping:: TrajectoryBuilderInterface *GetTrajectoryBuilder(int trajectory_id) const override {
return trajectory_builders_.at(trajectory_id).get();
}
collated_trajectory_builder.h
// Collates sensor data using a sensor::CollatorInterface, then passes it on to
// a mapping::TrajectoryBuilderInterface
// 收集传感器数据，然后传给mapping::TracjectoryBuilderInterface
CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* sensor_collator, int trajectory_id,
const std::set& expected_sensor_ids,
std::unique_ptr wrapped_trajectory_builder);
// 不同传感器的数据
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
AddData(sensor::MakeDispatchable(sensor_id, imu_data));
}
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
AddData(sensor::MakeDispatchable(sensor_id, odometry_data));
}
// 最后都传到trajectory builder
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const std::string& sensor_id, std::unique_ptr data) {
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}
template
class Dispatchable : public Data {
public:
Dispatchable(const std::string &sensor_id, const DataType &data)
: Data(sensor_id), data_(data) {}
common:: Time GetTime() const override { return data_.time; }
// sensor:: Data 最后还是调用trajectory_builder:: AddSensorData
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}
const DataType &data() const { return data_; }
private:
const DataType data_;
};
global_trajectory_builder.cc
template
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
// 构造函数是一个local trajectory加上一个pose graph， 一个local slam result的callback函数
GlobalTrajectoryBuilder(
std::unique_ptr local_trajectory_builder,
const int trajectory_id, PoseGraph* const pose_graph,
const LocalSlamResultCallback& local_slam_result_callback)；
// 点云数据
void AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override {
std::unique_ptr
matching_result = local_trajectory_builder_->AddRangeData(
sensor_id, timed_point_cloud_data);
if (matching_result->insertion_result != nullptr) {
kLocalSlamInsertionResults->Increment();
// pose graph add node
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_);
insertion_result = absl::make_unique(InsertionResult{
node_id, matching_result->insertion_result->constant_data,
std::vector>(
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));
}
}
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
if (local_trajectory_builder_) {
local_trajectory_builder_->AddImuData(imu_data);
}
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
if (local_trajectory_builder_) {
local_trajectory_builder_->AddOdometryData(odometry_data);
}
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
}
local_trajectory_builder_2d.h
std::unique_ptr
LocalTrajectoryBuilder2D:: AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) {
auto synchronized_data =
range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
const common::Time current_sensor_time = synchronized_data.time;
absl::optional 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,
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment, sensor_duration);
}
}
std::unique_ptr
LocalTrajectoryBuilder2D:: AddAccumulatedRangeData(const common::Time time,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment,
const absl::optional& sensor_duration) { std::unique_ptr pose_estimate_2d =
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr) {
LOG(WARNING) << "Scan matching failed.";
return nullptr;
}
std::unique_ptr insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
return absl::make_unique(
MatchingResult{time, pose_estimate, std::move(range_data_in_local),
std::move(insertion_result)});
}