Pose_graph_2d

Cartographer – PoseGraph2D

PoseGraph2D 继承于PoseGraph, PoseGraph继承于PoseGraphInterface
PoseGraph2D 里面主要有两个部分OptimizationProblem2D, ThreadPool

class PoseGraph2D: public PoseGraph
{
public:
	PoseGraph2D( const proto:: PoseGraphOptions& options, std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem, common::ThreadPool* thread_pool);
	
 // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
  // determined by scan matching against 'insertion_submaps.front()' and the
  // node data was inserted into the 'insertion_submaps'. If
  // 'insertion_submaps.front().finished()' is 'true', data was inserted into
  // this submap for the last time.
  // 一个新的node 里面有 TrajectoryNode::Data> constant_data, 他的local_pose是在前端
  //与insertion_submaps做scan match得到的,并且把这个数据插入到insertion_submaps中, 
 // 如果这个insertion_submaps.front()完成,则将这个数据最后一次插入到submap中.可以通过
  //下面了解TrajectoryNode::Data的数据格式
  NodeId AddNode(  std::shared_ptr<const TrajectoryNode::Data> constant_data,
  				    int trajectory_id,
  				    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) EXCLUDES(mutex_);
  
};

NodeId PoseGraph2D::AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data,
							 const int trajectory_id,
							 const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
 {
 	const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
 }

transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(const int trajectory_id) const
{
	  common::MutexLocker locker(&mutex_);
	  return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
}


transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
																	const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses ,
																	const int trajectory_id) const

{
  auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
  auto end_it   = global_submap_poses.EndOfTrajectory(trajectory_id);
  if (begin_it == end_it)
   {
    const auto it = initial_trajectory_poses_.find(trajectory_id);
    if (it != initial_trajectory_poses_.end()) 
    {
     	 return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
                                                 it->second.time) *it->second.relative_pose;   
    } 
    else
     {
     	 return transform::Rigid3d::Identity();
    }
  }
  const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
  // Accessing 'local_pose' in Submap is okay, since the member is const.
  return transform::Embed3D(global_submap_poses.at(last_optimized_submap_id).global_pose) *
  							  submap_data_.at(last_optimized_submap_id).submap->local_pose().inverse();
}

struct TrajectoryNode
 {
	  struct Data
	   {
		    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;
		    
		    // Used for loop closure in 3D.
		    sensor::PointCloud high_resolution_point_cloud;
		    sensor::PointCloud low_resolution_point_cloud;
		    Eigen::VectorXf rotational_scan_matcher_histogram;
		    
		    // 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;
	};

class PoseGraph: public PoseGraphInterface
{
public:
	struct InitialTrajectoryPose
	{
		int to_trajectory_id;
		transfrom::Rigid3d relative_pose;
		common::Time time;
	}
};

PoseGraphInterface中定义了
Constraint, LandmarkNode, SubmapPose, SubmapData, TrajectoryData, GlobalSlamOptimizationCallback

class PoseGraphInterface
{
public:
	struct Constraint
	{
		 struct Pose
		  {
		      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').
	    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
	};
	// LandmarkNode:: 一个观测包含相对位置,属于轨迹ID, 权重, 一个节点有很多个观测,和全局位置
	struct LandmarkNode
	 {
		    struct LandmarkObservation
		     {
			      int trajectory_id;
			      common::Time time;
			      transform::Rigid3d landmark_to_tracking_transform;
			      double translation_weight;
			      double rotation_weight;
	   	     };
    	     std::vector<LandmarkObservation> landmark_observations;
             common::optional<transform::Rigid3d> global_landmark_pose;
      };
	
	struct SubmapPose
	{
		int version;
		transform::Rigid3d pose;
	};
	struct SubmapData
	{
		std::shared_ptr<const Submap> submap;
		transform::Rigid3d pose;
	};
	struct TrajectoryData
	{
		double gravity_constant = 9.8;
		std::array<double, 4> imu_calibration{ {1., 0., 0., 0.} };
		common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
	};
	
	 using GlobalSlamOptimizationCallback =
     std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
                         const std::map<int /* trajectory_id */, NodeId>&)>; 
                         
    PoseGraphInterface() {}
  	virtual ~PoseGraphInterface() {}

    PoseGraphInterface(const PoseGraphInterface&) = delete;
    PoseGraphInterface& operator=(const PoseGraphInterface&) = delete;

	// Waits for all computations to finish and computes optimized poses.
	virtual void RunFinalOptimization() = 0;

    // Returns data for all submaps.
    virtual MapById<SubmapId, SubmapData> GetAllSubmapData() const = 0;

    // Returns the global poses for all submaps.
    virtual MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const = 0;

    // Returns the transform converting data in the local map frame (i.e. the
    // continuous, non-loop-closed frame) into the global map frame (i.e. the
    // discontinuous, loop-closed frame).
    virtual transform::Rigid3d GetLocalToGlobalTransform(
      int trajectory_id) const = 0;

    // Returns the current optimized trajectories.
    virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const = 0;

    // Returns the current optimized trajectory poses.
    virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
      const = 0;

    // Returns the current optimized landmark poses.
    virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses()
      const = 0;

    // Sets global pose of landmark 'landmark_id' to given 'global_pose'.
    virtual void SetLandmarkPose(const std::string& landmark_id,
                               const transform::Rigid3d& global_pose) = 0;

    // Checks if the given trajectory is finished.
     virtual bool IsTrajectoryFinished(int trajectory_id) const = 0;

    // Checks if the given trajectory is frozen.
    virtual bool IsTrajectoryFrozen(int trajectory_id) const = 0;

    // Returns the trajectory data.
    virtual std::map<int, TrajectoryData> GetTrajectoryData() const = 0;

    // Returns the collection of constraints.
    virtual std::vector<Constraint> constraints() const = 0;

    // Serializes the constraints and trajectories.
    virtual proto::PoseGraph ToProto() const = 0;

    // Sets the callback function that is invoked whenever the global optimization
    // problem is solved.
    virtual void SetGlobalSlamOptimizationCallback(
      GlobalSlamOptimizationCallback callback) = 0;                
};