Optimization_problem_2d.h

最新推荐文章于 2024-07-13 17:15:55 发布

小西服红色

最新推荐文章于 2024-07-13 17:15:55 发布

阅读量381

点赞数

分类专栏： Cartographer Graph Optimization

本文链接：https://blog.csdn.net/caizou123123/article/details/85163826

版权

Cartographer 同时被 2 个专栏收录

8 篇文章 2 订阅

订阅专栏

Graph Optimization

3 篇文章 1 订阅

订阅专栏

Optimization_problem_2d

struct NodeSpec2D
{
 	common::Time time;
 	transform::Rigid2d local_pose_2d;       //节点的局部位姿
 	transform::Rigid2d global_pose_2d;　　//节点的全局位姿
 	Eigen::Quaterniond gravity_alignment;　//重力对齐
};

 struct SubmapSpec2D
 {
 	transform::Rigid2d global_pose; //全局submap 姿态
 }

Implents the SPA loop closure method optimization_problem_interface.h

template <typename NodeDataType, typename SubmapDataType, typename RigidTransformsformType>
class OptimizationProblemInterface
{
public :
	using Constraint = PoseGraphInterface::Constraint;
	using LandmarkNode = PoseGraphInterface::LandmarkNode;
};

介绍Constraint && LandmarkNode

1. Constraint :: PoseGraphInterface

class PoseGraphInterface
{
	public:
	struct Constaint
	{
		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 pose;                            // Pose of the node j relative to submap i
   		enum Tag {INTRA_SUBMAP, 　INTER_SUBMAP } tag;
	};
}

图优化，就是将机器人的观测历史用一个图来表示(The idea is to represent the history of the robot measurements by a graph)，每个节点可以表示一次传感器的观测，或者一个局部地图，并且都带有一个观测时候的位置信息(Every node of the graph represents a sensor measurement or a local map and it is labeled with the location at which the measurement was taken)，　一个边就可以表示为空间的限制(An edge between two nodes encodes the spatial information arising from the alignment of the connected measurements and can be regarded as a spatial constraint between the two nodes).
基于图优化的ＳＬＡＭ，要考虑两个不同的问题，一个是，确认基于传感器数据的限制问题(The first one is to identify the constraints based on sensor data. This so-called ~~data association~~ problem is typically hard due to potential ambiguities or symmetries in the environment. A solution to this problem is often referred to as the SLAM ~~front-end~~ and it directly deals with the sensor data),第二个就是矫正机器人的姿态得到一个连续的地图(The second problem is to correct the poses of the robot to obtain a consistent map of the
environment given the constraints. This part of the approach is often referred to as the optimizer or the SLAM ~~back-end~~ . Its task is to seek for a configuration of the nodes that maximizes the likelihood of the measurements encoded in the constraints)
在这里插入图片描述

Sparse Pose Adjustment

利用Levenberg-Marquardt(LM) 方法作为优化框架，优化系统中的变量为全局位姿 $c_i = [t_i, \theta_i] = [x_i, y_i, \theta_i]^T$ , 一个限制就是从节点 $c_i$ 对节点 $c_j$ 的一次观测， $\bar{z}_{ij}$ 为观测偏移，带有精度矩阵\Lambda_{ij} (inverse of convariance), 对应于真实的偏移可以通过计算
$h(c_i, c_j) \equiv \left\{\begin{matrix} \Re_i^T(t_j-t_i) \\ \theta_j- \theta_i \end{matrix}\right.$

公式１

错误函数管理一个限制和整体错误可以表示：
$e_ij \equiv \bar{z}_{ij} - h(c_i, c_j)$
$\chi^2(c,p) \equiv \sum_{ij} e_{ij}^T\Lambda e_{ij}$

公式２

其中角度参数是不唯一的，由于加和减去 $2\pi$ 值一样，所以角度归一化 $(-\pi, \pi]$
SpaCostFunction2D

class SpaCostFunction2D
{
public:
	explicit SpaCostFunction2D(const PoseGraphInterface::Constraint::Pose& observed_relative_pose):
              						observed_relative_pose_(observed_relative_pose)
	{
	}
	// 重载()操作符求残差
    template <typename T> bool operator()(const T* const start_pose, const T* const end_pose, T*e) const
    {
    	const std::array<T, 3> error = ScaleError(ComputeUnscaleError(transform::Project2D(observed_relative_pose_.zbar_ij), start_pose, end_pose), observed_relative_pose_.translation_weight, observed_relative_pose_.rotation_weight);
        std::copy(std::begin(error), std::end(error), e);
        return true;
    }
 private:
  const PoseGraphInterface::Constraint::Pose observed_relative_pose_;	
};

ComputeUnscaleError：求节点残差

template<typename T> static std::array<T, 3> ComputeUnscaleError(const transform::Rigid2d& relative_pose, const T* const start, const T* const end)
{
	const T cos_theta_i = cos(start[2]); 
	const T sin_theta_i = sin(start[2]);
	const T delta_x = end[0] - start[0];
	const T delta_y = end[1] - start[1];
	//对应公式１
    const T h[3] = { cos_theta_i*delta_x + sin_theta_i*delta_y,
                     -sin_theta_i*delta_x + cos_theta_i*delta_y,
                     end[2] - start[2 }  
   // 对应公式２ 
   return  {{T( relative_pose.translation().x() - h[0],
              　 relative_pose.translation().y() - h[1],
               　common::NormalizeAngleDifference(T(relative_pose.rotation().angle() - h[2])}};

// Bring the 'difference' between two angles into [-pi; pi].
template <typename T>T NormalizeAngleDifference(T difference)
 {
      const T kPi = T(M_PI);
      while (difference > kPi) difference -= 2. * kPi;
      while (difference < -kPi) difference += 2. * kPi;
      return difference;
}




template <typename T>static std::array<T, 6> ComputeUnscaledError(const transform::Rigid3d& relative_pose,
																        const T* const start_rotation,
   																	 const T* const start_translation, 
   																	 const T* const end_rotation,
                                                                  const T* const end_translation)
 {
  	const Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1], -start_rotation[2],  -start_rotation[3]);
	const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0], 
	                                   end_translation[1] - start_translation[1],
                                       end_translation[2] - start_translation[2]);
                                       
    const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
    const Eigen::Quaternion<T> h_rotation_inverse = Eigen::Quaternion<T>(end_rotation[0], 
																		    -end_rotation[1], 
																		    -end_rotation[2],
																		    -end_rotation[3]) *
													   Eigen::Quaternion<T>(start_rotation[0],
																		     start_rotation[1],
																		     start_rotation[2], 
																		     start_rotation[3]);

  const Eigen::Matrix<T, 3, 1> angle_axis_difference = transform::RotationQuaternionToAngleAxisVector( h_rotation_inverse * relative_pose.rotation().cast<T>());

  return {{T(relative_pose.translation().x()) - h_translation[0],
           T(relative_pose.translation().y()) - h_translation[1],
           T(relative_pose.translation().z()) - h_translation[2],
           angle_axis_difference[0], angle_axis_difference[1],
           angle_axis_difference[2]}};
}

Scale Error: 对残差加上权重

template <typename T>
std::array<T, 3> ScaleError(const std::array<T, 3>& error,
                            double translation_weight, double rotation_weight)
{
     // clang-format off
     return {{ error[0] * translation_weight,
               error[1] * translation_weight,
               error[2] * rotation_weight }};
     // clang-format on
}

2. LandMarkNode:: PoseGraphInterface

一个landmark可以作为一个节点, 包括全局位置，观测(很多个vector)

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;
};

其中optional的定义：

template <class T> class optional
 {
 public:
		  optional() {}

		  optional(const optional& other)
		   {
			    if (other.has_value()) 
			    {
			      value_ = make_unique<T>(other.value());
			    }
		  }

          explicit optional(const T& value) { value_ = make_unique<T>(value); }

          bool has_value() const { return value_ != nullptr; }

         const T& value() const
          {
               CHECK(value_ != nullptr);
               return *value_;
          }

         optional<T>& operator=(const T& other_value) 
         {
             this->value_ = make_unique<T>(other_value);
             return *this;
        }

       optional<T>& operator=(const optional<T>& other)
       {
		    if (!other.has_value()) 
		    {
		      this->value_ = nullptr;
		    } 
		    else 
		    {
		      this->value_ = make_unique<T>(other.value());
		    }
		    return *this;
  	　　}

 private:
  std::unique_ptr<T> value_;
};

int main()
{
    optional<float> x(5.0);

    cout<<x.has_value()<<endl;
    cout<<x.value();
    return 0;
}

结果为：
optional的使用方法
介绍完constraint && LandmarkNode我们继续回到OptimizationProblemInterface中

class OptimizationProblemInterface {
 public:
 　　　 using Constraint = PoseGraphInterface::Constraint;　　　　　　//上面介绍
　　　  using LandmarkNode = PoseGraphInterface::LandmarkNode;　　//上面介绍

 　　　 OptimizationProblemInterface() {}
  　　　virtual ~OptimizationProblemInterface() {}

 　　　 OptimizationProblemInterface(const OptimizationProblemInterface&) = delete;
 　　　 OptimizationProblemInterface& operator=(const OptimizationProblemInterface&) =　delete;
 　　　// 参数有　trajectory_id, sensor::ImuData，　sensor::OdometryData, NodeDataType node_data
 　　　 virtual void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) = 0;
 　　　 virtual void AddOdomeetryData(int trajectory_id, const sensor::OdometryData& odometry_data) = 0;
 　　　 virtual void AddTrajectoryNode(int trajectory_id, const NodeDataType& node_data);
 　　　 virtual void AddSubmap(int trajectory_id, RigidTransformType& global_submap_pose) = 0;
 　　　 // 参数有NodeId  node_id, 
 　　　 virtual void InsertTrajectoryNode();
 　　　 virtual void TrimTrajectoryNode();
 　　　 //
 　　　
 　　　 virtual void InsertSubmap();
 　　　 virtual void TrimSubmap();
 　　　 virtual void SetMaxNumIterations();
       // Optimizes the global poses
       virtual void Solve( const std::vector<Constraint>& constraints, const std::set<int>& frozen_trajectories,const std::map<std::string, LandmarkNode>& landmark_nodes ) = 0;

分别从这里每个函数引申出代码和算法的原理细节．

1. ImuData

struct ImuData
{
	common::Time time;
	Eigen::Vector3d linear_acceleration;
	Eigen::Vector3d angular_velocity;
};

void AddImuData(const int trajectory_id, const sensor::ImuData& imu_data)

imu_data_.Append(trajectory_id, imu_data);
其中imu_data_的定义为：sensor::MapByTime<sensor::ImuData> 
MapByTime的定义：  std::map<int, std::map<common::Time, DataType>> data_;
data_的定义为　trajectory_id     　　　　　　　　　　　　　[map]
                   0                             time0     data
                                                 time1       data
                                                  :
                                                  :
  －－－－－－－－－－－－－－－－                 －－－－－－－
                　  1                           　time0   data
                                                　time1    data 
                                                　:
                                               　：     
templated<typename DataType> class MapByTime
{
public:
void Append(const int trajectory_id, const DataType& data)
{
	CHECK_GE(trajectory_id, 0);
	auto& trajectory = data_[trajectory_id];
	if(!trajectory.empty())
	{
		CHECK_GT(data.Time, std::prev(trajectory.end())->first);
		trajectory.emplace(data.time, data);
	}	
　}
　
}

2. OdometryData

struct OdometryData
{
	common::Time time;
	transform::Rigid3d pose;
}

void AddOdometryData(const int trajectory_id, const sensor::OdometryData& odometry_data)

odometry_data_.Append(trajectory_id, odometry_data);

3 Rigid3 和　Rigid2

template <typename FloatType> class Rigid3
{
	using Vector        = Eigen::Matrix<FloatType, 3, 1>;
	using Quaternion = Eigen::Quaternion<FloatType>;
	using AngleAxis   = Eigen::AngleAxis<FloatType>;
	// Rigid3包含　平移和旋转，其中旋转使用四元数表示或者 AngleAxis :rotation angle around an arbitrary 3D axis
	//其中AngleAxis可以给四元数Quaternion(AngleAxis)
	Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
	//主要函数求逆矩阵
	Rigid3 inverse() const
	{	
	 	const Quaternion rotation = rotation_.conjugate();
	 	const Vector translation = -(rotation*translation_);
	 	return Rigid3(translation, rotation)
	}

tmeplate <typename FloatType> class Rigid2
{
public:
	using Vector = Eigen::Matrix<FloatType, 2, 1>;
	//Eigen::Rotation2D : angle, ［cos, sin; -sin, cos］
	using Rotation2D = Eigen::Rotation2D<FloatType>;
	Rigid2 inverse() const
	{
		const Rotation2D rotation = rotation_.inverse();
		const Vector translation   = -(rotation*translation_);
		return Rigid2(translation, rotation);
	}
	double normalized_angle() const
	{
		return common::NormalizedAngleDifference(rotaion().angle());
	}	
}
	
};

4. NodeSpec2D && SubmapSpec2D

struct NodeSpec2D
{
	common::Time time;
	transform::Rigid2d local_pose_2d;
	transform::Rigid2d global_pose_2d;
	Eigen::Quaterniond gravity_alignment;
};

struct SubmapSpec2D
{
	transform::Rigid2d global_pose;
}

3. Ceres Solver

Ceres Solver 是谷歌开发的一款用于非线性优化的库,所以简单介绍下如何使用Ceres Solver教程
使用Ceres求解非线性优化问题一共分三个步骤, 第一步, 构建cost function目标函数,通过在结构体内重载()运算符;第二步, 通过代价函数构建待求解优化问题;第三步, 配置求解器参数并求解问题, 设置方程如何求解,是否输出求解过程,然后调用Solve方法.
具体可以参考下面例程:

#include <iostream>
#include <ceres/ceres.h>
using namespace std;
using namespace ceres;
// 第一步:构建代价函数重载()
struct CostFunctor
{
	//待优化变量x, 残差residual(代价函数的输出)
	template<typename T> bool operator()(const T* const x, T* residual) const
	{
		residual[0] = T(10.0) - x[0];
		return true;
	}
};

int main(int argc, char** argv)
{
	double initial_x = 5.0;
	double x = initial_x;
	//第二部分:构建寻优问题
	Problem problem;
	CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); // 使用自动求导,将之前的代价函数结构体传入,1,残差维度,2 输入维度,即待寻优参数x的维度
	//第三部分:
	Solver::Options options;
	options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
	options.minimizer_progress_to_stdout = true;
	Solver::Summary summary;//优化信息
	Solve(options, &problem, &summary); //求解
	std::cout<<summary.BriefReport()<<"\n"; //输出优化的简要信息
	//最终结果
	std::cout<<"x: <<initial_x<<"->"<<x<<endl;
	return 0;
}
使用的是自动求导(AutoDiffCostFunction) Ceres库中还有更多的求导方式,比如数值求导(NumeriacDiffCostFunction)

在cartographer中大量使用了Ceres, ResidualBlock problem.AddResidualBlock,和ParameBlock problem.AddParmeterBlock

void OptimizationProblem2D::Solve(const std::vector<Constraint>& constraints,  
                                  const std::set<int>& frozen_trajectories,
                                  const std::map<std::string, LandmarkNode>& landmark_nodes)
{
	  if (node_data_.empty())
	  {
	   	 // Nothing to optimize.
	   	 return;
	  }

      ceres::Problem::Options problem_options;
      ceres::Problem problem(problem_options);
      // Set the starting point.
      // TODO(hrapp): Move ceres data into SubmapSpec.
      MapById<SubmapId, std::array<double, 3>> C_submaps;
      MapById<NodeId, std::array<double, 3>> C_nodes;
      std::map<std::string, CeresPose> C_landmarks;

NodeId, SubmapId, MapById

//Uniquely identifies a trajectory node using a combination of a unique
// trajectory ID and a zero-based index of the node inside that trajectory.
struct NodeId 
 {
 	 int trajectory_id;
  	 int node_index;
     bool operator==(const NodeId& other) const
     {
   		 	return std::forward_as_tuple(trajectory_id, node_index) ==
        	std::forward_as_tuple(other.trajectory_id, other.node_index);
	  }

 	 bool operator!=(const NodeId& other) const { return !operator==(other); }

     bool operator<(const NodeId& other) const
     {
          return std::forward_as_tuple(trajectory_id, node_index) <
                 std::forward_as_tuple(other.trajectory_id, other.node_index);
  }

  void ToProto(proto::NodeId* proto) const
  {
   	  proto->set_trajectory_id(trajectory_id);
      proto->set_node_index(node_index);
  }
};

template <typename IdType, typename DataType>
class MapById 
{
 private:
      struct MapByIndex;
 public:
      struct IdDataReference 
      {
     	     IdType id;
   		     const DataType& data;
      };
      
	class ConstIterator 
	{
	
	};
	
 private:
  struct MapByIndex
  {
      bool can_append_ = true;
      std::map<int, DataType> data_;
  };
  static int GetIndex(const NodeId& id) { return id.node_index; }
  static int GetIndex(const SubmapId& id) { return id.submap_index; }
  std::map<int, MapByIndex> trajectories_;

   MapById<SubmapId, std::array<double, 3>> C_submaps;
   MapById<NodeId, std::array<double, 3>> C_nodes;
   就是C_submaps :   trajectory_id      MapByIndex
                            0               map<int, std::array<double, 3>data_
       C_nodes   :          0               map<int, std::array<double, 3>data_

接着Solve函数中定义

std::map<std::string, CeresPose> C_landmarks;
class CeresPose 
{
 public:
	  CeresPose(const transform::Rigid3d& rigid,
	            std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
	            std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
	            ceres::Problem* problem);

  const transform::Rigid3d ToRigid() const;

  double* translation() { return data_->translation.data(); }
  const double* translation() const { return data_->translation.data(); }

  double* rotation() { return data_->rotation.data(); }
  const double* rotation() const { return data_->rotation.data(); }

  struct Data 
  {
	    std::array<double, 3> translation;
	    // Rotation quaternion as (w, x, y, z).
	    std::array<double, 4> rotation;
  };

  Data& data() { return *data_; }

 private:
  std::shared_ptr<Data> data_;
};

submap_data_ :MapById<SubmapId, SubmapSpec2D>

for (const auto& submap_id_data : submap_data_)
 {
	    const bool frozen = frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
	    // submap_id_data .SubmapId. trajectory_id
	    //submap_id_data.data (SubmapSpec2D)
	    C_submaps.Insert(submap_id_data.id, FromPose(submap_id_data.data.global_pose));
	   //把submap 的globalpose作为优化参数 3个变量
	   
	    problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
	    
	    //第一个Submap作为固定的不进行优化
	    if (first_submap || frozen) 
	    {
		      first_submap = false;
		      // Fix the pose of the first submap or all submaps of a frozen
		      // trajectory.
		      problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
	    }
  }

  for (const auto& node_id_data : node_data_) 
  {
	    const bool frozen = frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
	    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
	    problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
	    if (frozen) 
	    {
	      	problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
	    }
  }

// Add cost functions for intra- and inter-submap constraints.

for (const Constraint& constraint : constraints)
 {
	     // Only loop closure constraints should have a loss function.
	    problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint.pose),
	                             constraint.tag == Constraint::INTER_SUBMAP? new ceres::HuberLoss(options_.huber_scale()) : nullptr,
	                             C_submaps.at(constraint.submap_id).data(),
	                             C_nodes.at(constraint.node_id).data());
  }

Add cost functions for landmarks.

  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, &C_nodes, &C_landmarks, &problem);

landmark_cost_function_2d

class LandmarkCostFunction2D
{
public:
	using LandmarkObservation = PoseGraphInterface::LandmarkNode::LandmarkObservation;
	static ceres::CostFunction* CreateAutoDiffCostFunction(const LandmarkObservation& observation, const NodeSpec2D& prev_node, const NodeSpec2D& next_node)
	{
		/* 6 residuals */
		/* 3 previous node pose variables */
		/* 3 next node pose variables */
		/* 4 landmark rotation variables */
		/* 3 landmark translation variables */
		return new ceres::AutoDiffCostFunction<LandmarkCostFunction2D, 6, 3, 3, 4, 3>(new LandmarkCostFunction2D(observation, prev_node, next_node));
	}
	
	template<typename T> bool operator()(const T* const prev_node_pose, const T* const next_node_pose, const T* const landmark_rotation, const T* const landmark_translation, T* const e) const
	{
		const std::tuple<std::array<T, 4>, std::array<T, 3> > interpolated_rotation_and_translation = InterpolateNodes2D( prev_node_pose, prev_node_gravity_alignment_, next_node_pose, next_node_gravity_alignment_, interpolation_parameter_);
		const std::array<T, 6>error = ScaleError(  ComputeUnscaleError( landmark_to_tracking_transform_, std::get<0>(interpolated_rotation_and_translation).data(),
		std::get<1>(interpolated_rotation_and_translation_.data(),
		landmark_rotation, landmark_translation),translation_weight_, rotation_weight_);
		std::copy(std::begin(error), std::end(error), e);
		return true;
	}
};

小西服红色

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
Optimization_problem_2d.h

Optimization_problem_2dstruct NodeSpec2D{ common::Time time; transform::Rigid2d local_pose_2d; //节点的局部位姿 transform::Rigid2d global_pose_2d;　　//节点的全局位姿 Eigen::Quaterniond gravity_alignm...
复制链接

扫一扫