devbox
运维做开发
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

cartographer landmark优化问题_computeunscalederror

作者：运维做开发 | 2024-07-24 21:46:34

赞

踩

computeunscalederror

一、添加landmark代价中涉及到的类型

  1. void AddLandmarkCostFunctions(
  2.     const std::map<std::string, LandmarkNode>& landmark_nodes,
  3.     bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
  4.     MapById<NodeId, std::array<double, 3>>* C_nodes,
  5.     std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
  6.     double huber_scale)

1. std::map<std::string, LandmarkNode>

2.LandmarkNode  

using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;

3.LandmarkNode struct

  1.   struct LandmarkNode {
  2.     struct LandmarkObservation {
  3.       int trajectory_id;
  4.       common::Time time;
  5.       transform::Rigid3d landmark_to_tracking_transform;
  6.       double translation_weight;
  7.       double rotation_weight;
  8.     };
  9.     std::vector<LandmarkObservation> landmark_observations;
  10.     absl::optional<transform::Rigid3d> global_landmark_pose;
  11.   };

2、具体形式调用

1.pose_graph_2d.cc   void PoseGraph2D::RunOptimization()函数中调用：

  1.   optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
  2.                                data_.landmark_nodes);

参数：：data_.constraints  所有数据的约束

GetTrajectoryStates()  轨迹的状态 ACTIVE, FINISHED, FROZEN, DELETED

data_.landmark_nodes   landmark的所有数据

2.optimization_problem_2d.cc    函数：

  1. void OptimizationProblem2D::Solve(
  2.     const std::vector<Constraint>& constraints,
  3.     const std::map<int, PoseGraphInterface::TrajectoryState>&
  4.         trajectories_state,
  5.     const std::map<std::string, LandmarkNode>& landmark_nodes)

中调用：

  1.   // Add cost functions for landmarks.
  2.   AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
  3.                            &C_nodes, &C_landmarks, &problem,
  4.                            options_.huber_scale());

参数：：landmark_nodes    ==data_.landmark_nodes  landmark的所有数据

freeze_landmarks = !frozen_trajectories.empty();   根据轨迹是否冻结判断

node_data_   MapById<NodeId, NodeSpec2D>   激光数据相对应的机器人位子

  1. struct NodeId {
  2.   NodeId(int trajectory_id, int node_index)
  3.       : trajectory_id(trajectory_id), node_index(node_index) {}
  4.  
  5.   int trajectory_id;
  6.   int node_index;
  7.   ...  == != < ToProto
  8. };
  9. struct NodeSpec2D {
  10.   common::Time time;
  11.   transform::Rigid2d local_pose_2d;
  12.   transform::Rigid2d global_pose_2d;
  13.   Eigen::Quaterniond gravity_alignment;
  14. };

&C_nodes, &C_landmarks,  是  node 局部位姿是输入，可得的全局位子。   landmark 优化后得到新位姿

  1. MapById<SubmapId, std::array<double, 3>> C_submaps;
  2. MapById<NodeId, std::array<double, 3>> C_nodes;
  3. std::map<std::string, CeresPose> C_landmarks;
  4. class CeresPose {
  5.  public:
  6.   CeresPose(
  7.       const transform::Rigid3d& rigid,
  8.       std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
  9.       std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
  10.       ceres::Problem* problem);
  11.   const transform::Rigid3d ToRigid() const;
  12.   double* translation() { return data_->translation.data(); }
  13.   const double* translation() const { return data_->translation.data(); }
  14.   double* rotation() { return data_->rotation.data(); }
  15.   const double* rotation() const { return data_->rotation.data(); }
  16.   struct Data {
  17.     std::array<double, 3> translation;
  18.     // Rotation quaternion as (w, x, y, z).
  19.     std::array<double, 4> rotation;
  20.   };
  21.   Data& data() { return *data_; }
  22.  private:
  23.   std::shared_ptr<Data> data_;
  24. };

3.AddLandmarkCostFunctions

  1. void AddLandmarkCostFunctions(
  2.     const std::map<std::string, LandmarkNode>& landmark_nodes,
  3.     bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
  4.     MapById<NodeId, std::array<double, 3>>* C_nodes,
  5.     std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
  6.     double huber_scale)

1.遍历landmark

for (const auto& landmark_node : landmark_nodes)

2. for (const auto& observation : landmark_node.second.landmark_observations)

3.  在landmark观测之前和之后找到轨迹节点。  //next 为最后一个node_data的迭代器

auto next =node_data.lower_bound(observation.trajectory_id, observation.time); 

4.landmark观察已经建立，但是轨迹没有添加时  

if (next == node_data.EndOfTrajectory(observation.trajectory_id))  continue

5.if (next == begin_of_trajectory)  next = std::next(next);

6.auto prev = std::prev(next);

7.std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);   //观察到landmark时的前一帧node_data

 std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);   //观察到landmark时的后一帧 node_data

8. const transform::Rigid3d starting_point  = 观察到的landmarkPose  或者 选择与landmark观测最接近的轨迹节点位姿

9.C_landmarks->emplace( landmark_id，starting_point（局部位姿）

10.如果landmark冻结则 该landmark 平移旋转固定

11.创建不同的landmark代价函数

  1.       problem->AddResidualBlock(
  2.           LandmarkCostFunction2D::CreateAutoDiffCostFunction(
  3.               observation, prev->data, next->data),
  4.           new ceres::HuberLoss(huber_scale), prev_node_pose->data(),
  5.           next_node_pose->data(), C_landmarks->at(landmark_id).rotation(),
  6.           C_landmarks->at(landmark_id).translation());

12.上述中 CreateAutoDiffCostFunction函数

  1.   static ceres::CostFunction* CreateAutoDiffCostFunction(
  2.       const LandmarkObservation& observation, const NodeSpec2D& prev_node,
  3.       const NodeSpec2D& next_node) {
  4.     return new ceres::AutoDiffCostFunction<
  5.         LandmarkCostFunction2D, 6 /* residuals */,
  6.         3 /* previous node pose variables */, 3 /* next node pose variables */,
  7.         4 /* landmark rotation variables */,
  8.         3 /* landmark translation variables */>(
  9.         new LandmarkCostFunction2D(observation, prev_node, next_node));
  10.   }

LandmarkCostFunction2D 函数

  1.   LandmarkCostFunction2D(const LandmarkObservation& observation,
  2.                          const NodeSpec2D& prev_node,
  3.                          const NodeSpec2D& next_node)
  4.       : landmark_to_tracking_transform_(
  5.             observation.landmark_to_tracking_transform),
  6.         prev_node_gravity_alignment_(prev_node.gravity_alignment),
  7.         next_node_gravity_alignment_(next_node.gravity_alignment),
  8.         translation_weight_(observation.translation_weight),
  9.         rotation_weight_(observation.rotation_weight),
  10.         interpolation_parameter_(
  11.             common::ToSeconds(observation.time - prev_node.time) /
  12.             common::ToSeconds(next_node.time - prev_node.time)) {}

该类中

  1.   template <typename T>
  2.   bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
  3.                   const T* const landmark_rotation,
  4.                   const T* const landmark_translation, T* const e) const {
  5.     const std::tuple<std::array<T, 4>, std::array<T, 3>>
  6.         interpolated_rotation_and_translation = InterpolateNodes2D(
  7.             prev_node_pose, prev_node_gravity_alignment_, next_node_pose,
  8.             next_node_gravity_alignment_, interpolation_parameter_);
  9.     const std::array<T, 6> error = ScaleError(
  10.         ComputeUnscaledError(
  11.             landmark_to_tracking_transform_,
  12.             std::get<0>(interpolated_rotation_and_translation).data(),
  13.             std::get<1>(interpolated_rotation_and_translation).data(),
  14.             landmark_rotation, landmark_translation),
  15.         translation_weight_, rotation_weight_);
  16.     std::copy(std::begin(error), std::end(error), e);
  17.     return true;
  18.   }

该类中 操作服()的定义中：

1>InterpolateNodes2D函数  计算两个插值的中间位姿

  1. template <typename T>
  2. std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
  3. InterpolateNodes2D(const T* const prev_node_pose,
  4.                    const Eigen::Quaterniond& prev_node_gravity_alignment,
  5.                    const T* const next_node_pose,
  6.                    const Eigen::Quaterniond& next_node_gravity_alignment,
  7.                    const double interpolation_parameter)

首先介绍InterpolateNodes2D函数 返回   旋转 4 ，平移 3维

 // The following is equivalent to (Embed3D(prev_node_pose) * Rigid3d::Rotation(prev_node_gravity_alignment)).rotation().

  1.   const Eigen::Quaternion<T> prev_quaternion(
  2.       (Eigen::AngleAxis<T>(prev_node_pose[2], Eigen::Matrix<T, 3, 1>::UnitZ()) *
  3.        prev_node_gravity_alignment.cast<T>())
  4.           .normalized());
  5.   const std::array<T, 4> prev_node_rotation = {
  6.       {prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(),
  7.        prev_quaternion.z()}};

同理  next_quaternion    next_node_rotation

返回角度：   SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(),  interpolation_parameter),

 就是  prev_scale * start[0] + next_scale * end[0]    T prev_scale(1. - factor);T next_scale(factor); 四元素的按比例插值

平移：    3维      {prev_node_pose[0] + interpolation_parameter * (next_node_pose[0] - prev_node_pose[0]),

   prev_node_pose[1] 同理   T(0)    相当于x y 前一个值+后一个值的比例

2>ComputeUnscaledError函数  计算參差

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)

首先求起始点旋转的逆R_i_inverse，然后求两个平移向量之差delta，求平移的最终差值 R_i_inverse * delta（相对与起始坐标），求旋转的逆，求角度的差值，求解最终的差值

  1. template <typename T>
  2. static std::array<T, 6> ComputeUnscaledError(
  3.     const transform::Rigid3d& relative_pose, const T* const start_rotation,
  4.     const T* const start_translation, const T* const end_rotation,
  5.     const T* const end_translation) {
  6.   const Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1],
  7.                                          -start_rotation[2],
  8.                                          -start_rotation[3]);
  9.  
  10.   const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0],
  11.                                      end_translation[1] - start_translation[1],
  12.                                      end_translation[2] - start_translation[2]);
  13.   const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
  14.  
  15.   const Eigen::Quaternion<T> h_rotation_inverse =
  16.       Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2],
  17.                            -end_rotation[3]) *
  18.       Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
  19.                            start_rotation[2], start_rotation[3]);
  20.  
  21.   const Eigen::Matrix<T, 3, 1> angle_axis_difference =
  22.       transform::RotationQuaternionToAngleAxisVector(
  23.           h_rotation_inverse * relative_pose.rotation().cast<T>());
  24.  
  25.   return {{T(relative_pose.translation().x()) - h_translation[0],
  26.            T(relative_pose.translation().y()) - h_translation[1],
  27.            T(relative_pose.translation().z()) - h_translation[2],
  28.            angle_axis_difference[0], angle_axis_difference[1],
  29.            angle_axis_difference[2]}};
  30. }

2>ScaleError函数  加上权重计算   

平移和旋转分别乘以其权重

将观察到的 landmark 计算所有的參差  最小二乘跟其他一起进行优化

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/运维做开发/article/detail/876871
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号