牛顿法的思路是将函数 f 在 x 处展开为多元二次函数，再通过求解二次函数最小值的方法得到本次迭代的下降方向。那么问题来了，多元二次函数在梯度为0的地方一定存在最小值么？直觉告诉我们是不一定的。以一元二次函数 g(x)=ax2+bx+c为例，我们知道当a>0时，g(x)可以取得最小值，否则g(x)不存在最小值。
如果在global_submap_poses_2d没有找到该轨迹的submap信息，那就说明该轨迹还没有产生submap，那就从data_.initial_trajectory_poses中查找该轨迹的初始位姿，如果找到 了初始位姿，就根据该轨迹初始位姿的时间去节点列表中找改时间点最近的节点，根据该节点的global_pose找到 local 和 global 的转换信息。
structPoseGraphData { /* struct InternalSubmapData { std::shared_ptr<const Submap> submap; SubmapState state = SubmapState::kNoConstraintSearch; // IDs of the nodes that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'kFinished'. std::set<NodeId> node_ids; }; */
// Submaps get assigned an ID and state as soon as they are seen, even // before they take part in the background computations. MapById<SubmapId, InternalSubmapData> submap_data;
// Global submap poses currently used for displaying data. MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d; MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;
// Data that are currently being shown. MapById<NodeId, TrajectoryNode> trajectory_nodes;
// Global landmark poses with all observations. std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode> landmark_nodes; // How our various trajectories are related. TrajectoryConnectivityState trajectory_connectivity_state; int num_trajectory_nodes = 0; std::map<int, InternalTrajectoryState> trajectories_state;
// Set of all initial trajectory poses. std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;
structInternalSubmapData { std::shared_ptr<const Submap> submap; SubmapState state = SubmapState::kNoConstraintSearch; // IDs of the nodes that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'kFinished'. std::set<NodeId> node_ids; };
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
structConstraint { structPose { transform::Rigid3d zbar_ij; double translation_weight; double rotation_weight; }; SubmapId submap_id; // 'i' in the paper. NodeId node_id; // 'j' in the paper. // Pose of the node 'j' relative to submap 'i'. Pose pose; /* Differentiates between intra-submap (where node 'j' was inserted into submap 'i') and inter-submap constraints (where node 'j' was not inserted into submap 'i').*/ enumTag { INTRA_SUBMAP, INTER_SUBMAP } tag; };
// Rays begin at 'origin'. 'returns' are the points where obstructions were // detected. 'misses' are points in the direction of rays for which no return // was detected, and were inserted at a configured distance // It is assumed that between the 'origin' and 'misses' is free space structRangeData { Eigen::Vector3f origin; PointCloud returns; PointCloud misses; };
using PointCloud = std::vector<RangefinderPoint>; // Stores 3D position of a point observed by a rangefinder sensor. structRangefinderPoint { Eigen::Vector3f position; };
structTrajectoryNode { structData { common::Time time; // Transform to approximately gravity align the tracking frame as // determined by local SLAM. Eigen::Quaterniond gravity_alignment; // Used for loop closure in 2D: voxel filtered returns in the // 'gravity_alignment' frame. sensor::PointCloud filtered_gravity_aligned_point_cloud; // 省略 3D.
// The node pose in the local SLAM frame. transform::Rigid3d local_pose; }; common::Time time()const{ return constant_data->time; } // This must be a shared_ptr. If the data is used for visualization while the // node is being trimmed, it must survive until all use finishes. std::shared_ptr<const Data> constant_data; // The node pose in the global SLAM frame. transform::Rigid3d global_pose; };
// An individual submap, which has a 'local_pose' in the local map frame, // keeps track of how many range data were inserted into it // and sets 'insertion_finished' when the map no longer changes // and is ready for loop closing classSubmap { public: Submap(const transform::Rigid3d& local_submap_pose) : local_pose_(local_submap_pose) {}
private: // Pose of this submap in the local map frame const transform::Rigid3d local_pose_; // Number of RangeData inserted. int num_range_data_ = 0; bool insertion_finished_ = false; };
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) { // 检测该点云数据 sensor_id 是否在期望的sensor_ids里面 CHECK_NE(expected_sensor_ids_.count(sensor_id), 0); // TODO(gaschler): These two cases can probably be one. // 此传感器类型数据已有 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). /* 如果当前融合时间段内融合结果中已包含同一ID的传感器数据，则应采用最新的点云数据进行替换 但是结束的时间戳仍然采用原来的时间刻度，开始进行数据截取合并CropAndMerge()并返回 */ // std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_; current_end_ = id_to_pending_data_.at(sensor_id).time; auto result = CropAndMerge(); id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); return result; } id_to_pending_data_.emplace(sensor_id, 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 (constauto& pair : id_to_pending_data_) { oldest_timestamp = std::min(oldest_timestamp, pair.second.time); } /* current_end_是下次融合的开始时间，是本次融合的最后时间刻度 但其实是此次融合所有传感器中最早的时间戳*/ current_end_ = oldest_timestamp; returnCropAndMerge(); }
