cartographer程式碼閱讀筆記
阿新 • • 發佈:2020-08-02
map_builder.h
是演算法的入口,封裝了local submap和 global pose-graph的優化
int MapBuilder:: AddTrajectoryBuiler() { std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( trajectory_options.trajectory_builder_2d_options(), SelectRangeSensorIds(expected_sensor_ids)); } DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get())); // 建立global trajectory的builder trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(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<SensorId>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> 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<sensor::Data> data) { data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); }
template <typename DataType> 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 <typename LocalTrajectoryBuilder, typename PoseGraph>
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
// 建構函式是一個local trajectory加上一個pose graph, 一個local slam result的callback函式
GlobalTrajectoryBuilder(
std::unique_ptr<LocalTrajectoryBuilder> 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<typename LocalTrajectoryBuilder::MatchingResult>
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>(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));
}
}
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:: MatchingResult>
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<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,
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment, sensor_duration);
}
}
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) { 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;
}
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
return absl::make_unique<MatchingResult>(
MatchingResult{time, pose_estimate, std::move(range_data_in_local),
std::move(insertion_result)});
}