diff --git a/src/cartographer/cartographer/carto.md b/src/cartographer/cartographer/carto.md index b5fccd7..310b94a 100644 --- a/src/cartographer/cartographer/carto.md +++ b/src/cartographer/cartographer/carto.md @@ -78,3 +78,15 @@ 6. ground_truth 7. metrics 8. cloud + +先讲 map_builder.cc + +照着sensor_bridge讲 CollatedTrajectoryBuilder 有个回调函数 + +再讲 sensor::Collator + +OrderedMultiQueue + +BlockingQueue + +最后 GlobalTrajectoryBuilder diff --git a/src/cartographer/cartographer/io/submap_painter.cc b/src/cartographer/cartographer/io/submap_painter.cc index 45f086d..d975b0c 100644 --- a/src/cartographer/cartographer/io/submap_painter.cc +++ b/src/cartographer/cartographer/io/submap_painter.cc @@ -273,7 +273,7 @@ UniqueCairoSurfacePtr DrawTexture(const std::vector& intensity, const uint8_t alpha_value = alpha.at(i); const uint8_t observed = (intensity_value == 0 && alpha_value == 0) ? 0 : 255; - + // tag: 这里需要确认一下 cairo_data->push_back((alpha_value << 24) | // 第一字节 存储透明度 (intensity_value << 16) | // 第二字节 存储栅格值 (observed << 8) | // 第三字节 存储是否被更新过 diff --git a/src/cartographer/cartographer/mapping/2d/grid_2d.cc b/src/cartographer/cartographer/mapping/2d/grid_2d.cc index 3dc6f7f..965d5b0 100644 --- a/src/cartographer/cartographer/mapping/2d/grid_2d.cc +++ b/src/cartographer/cartographer/mapping/2d/grid_2d.cc @@ -115,6 +115,7 @@ void Grid2D::FinishUpdate() { // Fills in 'offset' and 'limits' to define a subregion of that contains all // known cells. +// 根据known_cells_box_更新limits void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, CellLimits* const limits) const { if (known_cells_box_.isEmpty()) { diff --git a/src/cartographer/cartographer/mapping/2d/probability_grid.cc b/src/cartographer/cartographer/mapping/2d/probability_grid.cc index d733d70..5490895 100644 --- a/src/cartographer/cartographer/mapping/2d/probability_grid.cc +++ b/src/cartographer/cartographer/mapping/2d/probability_grid.cc @@ -114,15 +114,17 @@ std::unique_ptr ProbabilityGrid::ComputeCroppedGrid() const { return std::unique_ptr(cropped_grid.release()); } -// todo: ProbabilityGrid::DrawToSubmapTexture +// 获取压缩后的地图栅格数据 bool ProbabilityGrid::DrawToSubmapTexture( proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const { Eigen::Array2i offset; CellLimits cell_limits; + // 根据known_cells_box_更新limits ComputeCroppedLimits(&offset, &cell_limits); std::string cells; + // 遍历地图, 将栅格数据存入cells for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { if (!IsKnown(xy_index + offset)) { cells.push_back(0 /* unknown log odds value */); @@ -135,11 +137,17 @@ bool ProbabilityGrid::DrawToSubmapTexture( // zero, and use 'alpha' to subtract. This is only correct when the pixel // is currently white, so walls will look too gray. This should be hard to // detect visually for the user, though. + // 我们想添加 'delta',但使用值和 alpha 是不可能的 + // 我们使用预乘 alpha,因此当 'delta' 为正时,我们可以通过将 'alpha' 设置为零来添加它。 + // 如果它是负数,我们将 'value' 设置为零,并使用 'alpha' 进行减法。 这仅在像素当前为白色时才正确,因此墙壁看起来太灰。 + // 但是,这对于用户来说应该很难在视觉上检测到。 + + // tag: 这里需要确认一下 const int delta = 128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset)); const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 value = delta > 0 ? delta : 0; - // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha + // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度 cells.push_back(value); cells.push_back((value || alpha) ? alpha : 1); } @@ -147,6 +155,7 @@ bool ProbabilityGrid::DrawToSubmapTexture( // 保存地图栅格数据时进行压缩 common::FastGzipString(cells, texture->mutable_cells()); + // 填充地图描述信息 texture->set_width(cell_limits.num_x_cells); texture->set_height(cell_limits.num_y_cells); const double resolution = limits().resolution(); diff --git a/src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h b/src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h index 5de358a..6271094 100644 --- a/src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h +++ b/src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h @@ -36,7 +36,6 @@ proto::ProbabilityGridRangeDataInserterOptions2D CreateProbabilityGridRangeDataInserterOptions2D( common::LuaParameterDictionary* parameter_dictionary); -// todo: ProbabilityGridRangeDataInserter2D class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface { public: explicit ProbabilityGridRangeDataInserter2D( diff --git a/src/cartographer/cartographer/mapping/2d/submap_2d.cc b/src/cartographer/cartographer/mapping/2d/submap_2d.cc index f6cec30..6491263 100644 --- a/src/cartographer/cartographer/mapping/2d/submap_2d.cc +++ b/src/cartographer/cartographer/mapping/2d/submap_2d.cc @@ -147,6 +147,7 @@ void Submap2D::ToResponseProto( proto::SubmapQuery::Response* const response) const { if (!grid_) return; response->set_submap_version(num_range_data()); + // note: const在*后边, 指针指向的地址不能变,而内存单元中的内容可变 proto::SubmapQuery::Response::SubmapTexture* const texture = response->add_textures(); // 填充压缩后的数据 diff --git a/src/cartographer/cartographer/mapping/internal/2d/local_slam_result_2d.h b/src/cartographer/cartographer/mapping/internal/2d/local_slam_result_2d.h index dbe0f0d..71b4f18 100644 --- a/src/cartographer/cartographer/mapping/internal/2d/local_slam_result_2d.h +++ b/src/cartographer/cartographer/mapping/internal/2d/local_slam_result_2d.h @@ -24,7 +24,6 @@ namespace cartographer { namespace mapping { -// tag: LocalSlamResult2D class LocalSlamResult2D : public LocalSlamResultData { public: LocalSlamResult2D( diff --git a/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index c52d1b1..562d9c2 100644 --- a/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -296,7 +296,7 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( sensor::RangeData range_data_in_local = TransformRangeData(gravity_aligned_range_data, transform::Embed3D(pose_estimate_2d->cast())); - + // 将处理后的雷达数据写入submap std::unique_ptr insertion_result = InsertIntoSubmap( time, range_data_in_local, filtered_gravity_aligned_point_cloud, pose_estimate, gravity_alignment.rotation()); @@ -328,7 +328,7 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( } /** - * @brief + * @brief 将处理后的雷达数据写入submap * * @param[in] time * @param[in] range_data_in_local diff --git a/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h b/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h index 38f8649..faed1df 100644 --- a/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h +++ b/src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h @@ -41,10 +41,8 @@ namespace mapping { // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) // without loop closure. // TODO(gaschler): Add test for this class similar to the 3D test. -// todo: LocalTrajectoryBuilder2D class LocalTrajectoryBuilder2D { public: - // tag: LocalTrajectoryBuilder2D::InsertionResult struct InsertionResult { std::shared_ptr constant_data; std::vector> insertion_submaps; diff --git a/src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc b/src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc index 6b68e94..6d13014 100644 --- a/src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -663,7 +663,6 @@ void PoseGraph2D::HandleWorkQueue( // DeleteTrajectoriesIfNeeded(); - // ?: TrimmingHandle TrimmingHandle trimming_handle(this); for (auto& trimmer : trimmers_) { trimmer->Trim(&trimming_handle); @@ -853,6 +852,7 @@ bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const { TrajectoryState::FINISHED; } +// 将指定轨迹id设置为FROZEN状态 void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { { absl::MutexLock locker(&mutex_); @@ -882,7 +882,7 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { }); } -// 返回轨迹是否是 FROZEN 状态 +// 判断轨迹是否是 FROZEN 状态 bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const { return data_.trajectories_state.count(trajectory_id) != 0 && data_.trajectories_state.at(trajectory_id).state == diff --git a/src/cartographer/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/src/cartographer/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index 3a49451..ced927a 100644 --- a/src/cartographer/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/src/cartographer/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -461,7 +461,6 @@ void OptimizationProblem2D::Solve( } // Step: 第五种残差 - // ?: 节点与gps第一帧间的坐标变换 problem.AddResidualBlock( CreateAutoDiffSpaCostFunction(constraint_pose), options_.fixed_frame_pose_use_tolerant_loss() diff --git a/src/cartographer/cartographer/mapping/internal/trajectory_connectivity_state.h b/src/cartographer/cartographer/mapping/internal/trajectory_connectivity_state.h index 99f5628..e55f0b5 100644 --- a/src/cartographer/cartographer/mapping/internal/trajectory_connectivity_state.h +++ b/src/cartographer/cartographer/mapping/internal/trajectory_connectivity_state.h @@ -29,7 +29,6 @@ namespace mapping { // // This class is thread-compatible. // 描述不同轨迹之间的连接状态 -// todo: TrajectoryConnectivityState class TrajectoryConnectivityState { public: TrajectoryConnectivityState() {} diff --git a/src/cartographer/cartographer/mapping/map_builder.cc b/src/cartographer/cartographer/mapping/map_builder.cc index a2efe4d..cc15fad 100644 --- a/src/cartographer/cartographer/mapping/map_builder.cc +++ b/src/cartographer/cartographer/mapping/map_builder.cc @@ -236,7 +236,7 @@ int MapBuilder::AddTrajectoryBuilder( return trajectory_id; } -// 从序列化的数据中构造一条 trajectory, 没有使用 +// 从序列化的数据中构造一条 trajectory int MapBuilder::AddTrajectoryForDeserialization( const proto::TrajectoryBuilderOptionsWithSensorIds& options_with_sensor_ids_proto) { @@ -307,24 +307,32 @@ std::map MapBuilder::LoadState( const auto& all_builder_options_proto = deserializer.all_trajectory_builder_options(); + // key为pbstream文件中的轨迹id, value为新生成的轨迹的id std::map trajectory_remapping; + + // 从文件中添加轨迹 for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i) { auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i); const auto& options_with_sensor_ids_proto = all_builder_options_proto.options_with_sensor_ids(i); + // 添加新轨迹 const int new_trajectory_id = AddTrajectoryForDeserialization(options_with_sensor_ids_proto); + // 原始轨迹id与新生成的轨迹id组成map,放入trajectory_remapping中 CHECK(trajectory_remapping .emplace(trajectory_proto.trajectory_id(), new_trajectory_id) .second) << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id(); + // 将原始id设置为新生成的id trajectory_proto.set_trajectory_id(new_trajectory_id); if (load_frozen_state) { + // 将指定轨迹id设置为FROZEN状态 pose_graph_->FreezeTrajectory(new_trajectory_id); } } // Apply the calculated remapping to constraints in the pose graph proto. + // 更新约束中节点与子图的轨迹id for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) { constraint_proto.mutable_submap_id()->set_trajectory_id( trajectory_remapping.at(constraint_proto.submap_id().trajectory_id())); @@ -332,6 +340,7 @@ std::map MapBuilder::LoadState( trajectory_remapping.at(constraint_proto.node_id().trajectory_id())); } + // 获取submap_poses MapById submap_poses; for (const proto::Trajectory& trajectory_proto : pose_graph_proto.trajectory()) { @@ -343,6 +352,7 @@ std::map MapBuilder::LoadState( } } + // 获取node_poses MapById node_poses; for (const proto::Trajectory& trajectory_proto : pose_graph_proto.trajectory()) { @@ -354,6 +364,7 @@ std::map MapBuilder::LoadState( } // Set global poses of landmarks. + // 获取landmark_poses for (const auto& landmark : pose_graph_proto.landmark_poses()) { pose_graph_->SetLandmarkPose(landmark.landmark_id(), transform::ToRigid3(landmark.global_pose()), @@ -386,6 +397,7 @@ std::map MapBuilder::LoadState( proto.submap().submap_id().trajectory_id())); const SubmapId submap_id(proto.submap().submap_id().trajectory_id(), proto.submap().submap_id().submap_index()); + // 将submap添加到位姿图中 pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id), proto.submap()); break; @@ -396,12 +408,14 @@ std::map MapBuilder::LoadState( const NodeId node_id(proto.node().node_id().trajectory_id(), proto.node().node_id().node_index()); const transform::Rigid3d& node_pose = node_poses.at(node_id); + // 将Node添加到位姿图中 pose_graph_->AddNodeFromProto(node_pose, proto.node()); break; } case SerializedData::kTrajectoryData: { proto.mutable_trajectory_data()->set_trajectory_id( trajectory_remapping.at(proto.trajectory_data().trajectory_id())); + // 将TrajectoryData添加到位姿图中 pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data()); break; } @@ -441,6 +455,7 @@ std::map MapBuilder::LoadState( } } + // 添加子图的附属的节点 if (load_frozen_state) { // Add information about which nodes belong to which submap. // This is required, even without constraints. @@ -456,7 +471,8 @@ std::map MapBuilder::LoadState( SubmapId{constraint_proto.submap_id().trajectory_id(), constraint_proto.submap_id().submap_index()}); } - } else { + } + else { // When loading unfrozen trajectories, 'AddSerializedConstraints' will // take care of adding information about which nodes belong to which // submap. @@ -467,9 +483,10 @@ std::map MapBuilder::LoadState( return trajectory_remapping; } -// 从文件读取slam的各个状态 +// 从pbstream文件读取信息 std::map MapBuilder::LoadStateFromFile( const std::string& state_filename, const bool load_frozen_state) { + // 检查后缀名 const std::string suffix = ".pbstream"; if (state_filename.substr( std::max(state_filename.size() - suffix.size(), 0)) != suffix) { diff --git a/src/cartographer/cartographer/mapping/pose_graph_interface.h b/src/cartographer/cartographer/mapping/pose_graph_interface.h index 860ab99..994c07f 100644 --- a/src/cartographer/cartographer/mapping/pose_graph_interface.h +++ b/src/cartographer/cartographer/mapping/pose_graph_interface.h @@ -34,7 +34,6 @@ class PoseGraphInterface { // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010. - // tag: PoseGraphInterface::Constraint // 包含了子图的id, 节点的id, 节点j相对于子图i的坐标变换, 以及节点是在子图内还是子图外的标志 struct Constraint { struct Pose { @@ -78,7 +77,6 @@ class PoseGraphInterface { transform::Rigid3d pose; }; - // tag: PoseGraphInterface::TrajectoryData struct TrajectoryData { double gravity_constant = 9.8; std::array imu_calibration{{1., 0., 0., 0.}}; diff --git a/src/cartographer/cartographer/mapping/trajectory_builder_interface.h b/src/cartographer/cartographer/mapping/trajectory_builder_interface.h index 71fce45..00c7770 100644 --- a/src/cartographer/cartographer/mapping/trajectory_builder_interface.h +++ b/src/cartographer/cartographer/mapping/trajectory_builder_interface.h @@ -57,7 +57,6 @@ class LocalSlamResultData; */ class TrajectoryBuilderInterface { public: - // tag: TrajectoryBuilderInterface::InsertionResult struct InsertionResult { NodeId node_id; std::shared_ptr constant_data; diff --git a/src/cartographer/cartographer/sensor/internal/ordered_multi_queue.h b/src/cartographer/cartographer/sensor/internal/ordered_multi_queue.h index 0968a96..b9e865a 100644 --- a/src/cartographer/cartographer/sensor/internal/ordered_multi_queue.h +++ b/src/cartographer/cartographer/sensor/internal/ordered_multi_queue.h @@ -31,7 +31,6 @@ namespace cartographer { namespace sensor { -// tag: QueueKey struct QueueKey { int trajectory_id; // 轨迹id std::string sensor_id; // topic名字 diff --git a/src/cartographer/cartographer/sensor/internal/voxel_filter.cc b/src/cartographer/cartographer/sensor/internal/voxel_filter.cc index 1942072..a6e5c0b 100644 --- a/src/cartographer/cartographer/sensor/internal/voxel_filter.cc +++ b/src/cartographer/cartographer/sensor/internal/voxel_filter.cc @@ -138,7 +138,6 @@ std::vector VoxelFilter( [](const RangefinderPoint& point) { return point.position; }); } -// todo: VoxelFilter PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { const std::vector points_used = RandomizedVoxelFilterIndices( point_cloud.points(), resolution, diff --git a/src/cartographer/cartographer/sensor/map_by_time.h b/src/cartographer/cartographer/sensor/map_by_time.h index d01d53e..ab7e9a2 100644 --- a/src/cartographer/cartographer/sensor/map_by_time.h +++ b/src/cartographer/cartographer/sensor/map_by_time.h @@ -32,7 +32,6 @@ namespace cartographer { namespace sensor { // 'DataType' must contain a 'time' member of type common::Time. -// todo: MapByTime template class MapByTime { public: diff --git a/src/cartographer/cartographer/transform/rigid_transform.h b/src/cartographer/cartographer/transform/rigid_transform.h index e8ad817..8677067 100644 --- a/src/cartographer/cartographer/transform/rigid_transform.h +++ b/src/cartographer/cartographer/transform/rigid_transform.h @@ -74,7 +74,6 @@ class Rigid2 { // T = [R t] T^-1 = [R^-1 -R^-1 * t] // [0 1] [0 1 ] // R是旋转矩阵, 特殊正交群, 所以R^-1 = R^T - // tag: 这里手写一下推倒过程 Rigid2 inverse() const { const Rotation2D rotation = rotation_.inverse(); const Vector translation = -(rotation * translation_); diff --git a/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 0abb7eb..c5557fc 100644 --- a/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -151,7 +151,6 @@ int MapBuilderBridge::AddTrajectory( const ::cartographer::mapping::TrajectoryBuilderInterface:: InsertionResult>) { // 保存local slam 的结果数据 5个参数实际只用了4个 - // tag: MapBuilderBridge::OnLocalSlamResult OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); }); LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; diff --git a/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h b/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h index e9b7803..4fc2a57 100644 --- a/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -63,7 +63,6 @@ class MapBuilderBridge { * 它的z轴指向上方, 即重力加速度矢量指向-z方向, 即由加速度计测得的重力分量沿+z方向. */ - // tag: MapBuilderBridge::LocalTrajectoryData struct LocalTrajectoryData { // Contains the trajectory data received from local SLAM, after // it had processed accumulated 'range_data_in_local' and estimated diff --git a/src/cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc b/src/cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc index c482640..bdf94f6 100644 --- a/src/cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/src/cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -506,7 +506,7 @@ std::unique_ptr CreateOccupancyGridMsg( ? -1 : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.); - /* tag: CreateOccupancyGridMsg + /* note: 生成ROS兼容的栅格地图 * 像素值65-100的设置占用值为100,表示占用,代表障碍物 * 像素值0-19.6的设置占用值为0,表示空闲,代表可通过区域 * 像素值在中间的值保持不变,灰色 diff --git a/src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc b/src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc index 56b89fa..0154266 100644 --- a/src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -228,7 +228,6 @@ void SensorBridge::HandleLaserScan( CHECK_LE(points.points.back().time, 0.f); // TODO(gaschler): Use per-point time instead of subdivisions. - // tag: 参数 num_subdivisions_per_laser_scan // 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1 for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index =