ceres学习笔记(三)

阿里云国内75折回扣微信号：monov8

阿里云国际，腾讯云国际，低至75折。AWS 93折免费开户实名账号代冲值优惠多多微信号：monov8 飞机：@monov6

学习了example中pose_graph_3d的部分记录一下学习过程。

前言

翻译一下readme里面的内容

...

该示例还说明了如何将 Eigen 的几何模块与 Ceres 的自动微分功能结合使用。为了表示方向我们将使用 Eigen 的四元数它使用 Hamiltonian 约定但与 Ceres 的旋转表示相比具有不同的元素顺序。具体来说它们的区别在于标量分量 q_w 是第一个还是最后一个 Ceres 的四元数的元素顺序是 [q_w, q_x, q_y, q_z]而 Eigen 的四元数是 [q_x, q_y, q_z, q_w]。

该包定义了对 3 维位姿图优化问题进行建模所需的必要 Ceres 成本函数以及构建和解决问题的二进制文件。显示成本函数是为了指导目的并且可以通过使用需要更长的时间来实现的分析导数来加速。

运行 ---------- 此包包含一个可执行文件 `pose_graph_3d`它将读取问题定义文件。此可执行文件可以处理任何使用 g2o 格式的 3D 问题定义四元数用于方向表示。为不同格式如 TORO 或其他格式实施新阅读器会相对简单。 `pose_graph_3d` 将打印 Ceres 求解器的完整摘要然后将机器人的原始和优化姿势分别为 `poses_original.txt` 和 `poses_optimized.txt`输出到磁盘格式如下

pose_id x y z q_x q_y q_z q_w

其中 `pose_id` 是文件定义中对应的整数 ID。请注意文件将按 `pose_id` 的升序排序。
...

主要是对于ceres用于位姿优化的一个说明对于ceres和eigen关于四元数定义还有costfunction的设置。然后对于g2o文件内容的说明。example里面没有找到g2o文件的data最后在github里面翻到了链接如下

Datasets – Luca Carlone

一、pose_graph_3d_error_term.h部分的解析

pose_graph_3d将代价函数类的定义单独写了一个头文件pose_graph_3d_error_term.h。先对它进行学习。

翻译注释

计算两个姿势的误差项这两个姿势之间具有相对姿势测量值。带hat的变量是测量值。我们有两个位姿 x_a 和 x_b。通过传感器测量我们可以测量帧 B w.r.t 帧 A 的变换表示为 t_ab_hat。我们可以计算位姿的当前估计和测量之间的误差度量。
在这个公式中我们选择将刚性变换表示为哈密顿四元数 q 和位置 p。四元数顺序为 [x, y, z,w]。

估计的测量值是t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]

[ q_ab ]

[ q_a^{-1}* q_b ]

其中 ^{-1} 表示逆矩阵R(q) 是四元数的旋转矩阵。现在我们可以计算估计和测量转换之间的误差度量。对于方向误差我们将使用标准乘法误差结果为

error = [ p_ab - \hat{p}_ab ]

[ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]

其中 Vec(*) 返回四元数的向量虚部部分。由于测量具有与其准确性相关的不确定性我们将通过测量信息矩阵的平方根对误差进行加权

residuals = I^{1/2) * error 其中 I 是信息矩阵它是协方差的倒数。

下面对PoseGraph3dErrorTerm类进行解析。

 private:
  // The measurement for the position of B relative to A in the A frame.
  const Pose3d t_ab_measured_;
  // The square root of the measurement information matrix.
  const Eigen::Matrix<double, 6, 6> sqrt_information_;

有两个私有变量

t_ab_measured_表示在A下B相对于A的位姿。这里要看一下Pose3d的定义

struct Pose3d {
  Eigen::Vector3d p;
  Eigen::Quaterniond q;

  // The name of the data type in the g2o file format.
  static std::string name() { return "VERTEX_SE3:QUAT"; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

它包括一个数据类型是Eigen::Vector3d的p表示位置一个数据类型是Eigen::Quaterniond的q表示旋转。然后还定义了一个name函数返回VERTEX_SE3:QUAT是se3下的四元数。

sqrt_information_表示测量信息矩阵的平方根它的数据类型是doublesize是6*6。

然后是构造函数

  PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
                       const Eigen::Matrix<double, 6, 6>& sqrt_information)
      : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}

它将传入的两个参数分别赋值给两个私有变量。

接下来是关键的重载运算符operator():

  template <typename T>
  bool operator()(const T* const p_a_ptr,
                  const T* const q_a_ptr,
                  const T* const p_b_ptr,
                  const T* const q_b_ptr,
                  T* residuals_ptr) const

传入了5个参数注意这里全部是相对于世界坐标系下的

p_a_ptr表示a点的位置

q_a_ptr表示a点的旋转用四元数来表示

p_b_ptr表示b点的位置

q_b_ptr表示b点的旋转

residuals_ptr是最后算的残差。

    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
    Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);

    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
    Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);

这里使用了eigen里面的映射函数主要是为了数据转换将p_a_ptr等分别赋值给p_a等方便之后的运算。

    // Compute the relative transformation between the two frames.
    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;

计算a和b之间的位姿变换关系q_a_inverse表示q_a的逆使用conjugate函数来求。最后求出了q_ab_estimated表示从b变换到a的旋转矩阵。这里就相当于q_b是相对于世界坐标系下的旋转q_a是相当于世界坐标系下的旋转。然后q_a_inverse * q_b就算出了b在a坐标系下的旋转。

公式推导如上图所示。

    // Represent the displacement between the two frames in the A frame.
    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);

这是求出了从b到a的平移向量p_ab_estimated。同理p_b表示世界坐标系下b的位置p_a表示世界坐标系下a的位置然后p_ab_estimated就应该表示b点在a坐标系下平移向量。公式推导如下

比较清晰明确。

    // Compute the error between the two orientation estimates.
    Eigen::Quaternion<T> delta_q =
        t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();

这部分是计算从b到a的旋转部分的测量值和估计值之间的差这里运用了四元数的差来求旋转的角度差这部分没有找到什么资料在一篇博客里面看到了四元数的差只给出了公式但也没有给出推导的过程。

四元数的差、对数、指数、幂以及差值_Magic_Conch_Shell的博客-CSDN博客_四元数的差

    // Compute the residuals.
    // [ position         ]   [ delta_p          ]
    // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
    Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
    residuals.template block<3, 1>(0, 0) =
        p_ab_estimated - t_ab_measured_.p.template cast<T>();
    residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();

这部分就是给6*1的残差向量赋值前三行是位移部分的差值后三行是旋转部分的差值vec是取四元数的虚部这里不知道为什么乘了2。

    // Scale the residuals by the measurement uncertainty.
    residuals.applyOnTheLeft(sqrt_information_.template cast<T>());

这里是加上了信息矩阵。

  static ceres::CostFunction* Create(
      const Pose3d& t_ab_measured,
      const Eigen::Matrix<double, 6, 6>& sqrt_information) {
    return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
        new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
  }

这里是创建自动求导的代价函数是因为四元数不能使用常规方法来求导就使用了这个。其中

new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>

第一个6表示residuals是6维的

第二个3表示operator()函数哪里的p_a_ptr是3维的后面的类似。

二、common/read_g2o.h部分

这是一个处理g2o文件的头文件下面来简单解析一下。

结合一下g20文件的内容来看

// Reads a single pose from the input and inserts it into the map. Returns false
// if there is a duplicate entry.
template <typename Pose, typename Allocator>
bool ReadVertex(std::ifstream* infile,
                std::map<int, Pose, std::less<int>, Allocator>* poses) {
  int id;
  Pose pose;
  *infile >> id >> pose;

  // Ensure we don't have duplicate poses.
  if (poses->find(id) != poses->end()) {
    LOG(ERROR) << "Duplicate vertex with ID: " << id;
    return false;
  }
  (*poses)[id] = pose;

  return true;
}

阅读顶点部分通过传入的文件将分别传入id和pose数据结构里面。这里有一个if判断判断的内容是查看poses是否有id这里的处理很巧妙它希望的是判断是否有重复的位姿这里的poses应该还没有存入id和pose存入操作在下面find()stl函数所以如果发现有相同的就返回iterator值没有就会返回end()后面的iterator值所以判断是否等于如果不等于说明poses里面已经有key为id的数据说明重复输入了。然后有一个map数据格式的变量poses将id和pose赋予它。

// Reads the contraints between two vertices in the pose graph
template <typename Constraint, typename Allocator>
void ReadConstraint(std::ifstream* infile,
                    std::vector<Constraint, Allocator>* constraints) {
  Constraint constraint;
  *infile >> constraint;

  constraints->push_back(constraint);
}

阅读两点之间限制的边部分这里是将g2o中的两点之间限制的边部分传入具体得看main里面怎么写。

// In 3D, a vertex is defined as follows:
//
// VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
//
// where the quaternion is in Hamilton form.
// A constraint is defined as follows:
//
// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
//
// where I_ij is the (i, j)-th entry of the information matrix for the
// measurement. Only the upper-triangular part is stored. The measurement order
// is the delta position followed by the delta orientation.

这部分注释主要的意思是说明g2o不同行的格式。

开头为VERTEX_SE3:QUAT的表示第一个参数是序号后面3个是在世界坐标系下的位移值最后四个是四元数。

开头为EDGE_SE3:QUAT的表示由b到a坐标系下的相关值

第一个值和第二个值表示从第二个到第一个的变换

第三个值到第五个值表示从b到a坐标系下位移值的变换

第六个值到第十个值表示从b到a坐标系下旋转的变换以四元数表示

后面的就是信息矩阵的上半部分因为它是对称的所以存了一半。

template <typename Pose,
          typename Constraint,
          typename MapAllocator,
          typename VectorAllocator>
bool ReadG2oFile(const std::string& filename,
                 std::map<int, Pose, std::less<int>, MapAllocator>* poses,
                 std::vector<Constraint, VectorAllocator>* constraints)

这是阅读g2o文件

第一个参数是阅读的文件名

第二个参数是关于顶点的poses

第三个参数是关于两点之间的限制边constraints。

  poses->clear();
  constraints->clear();

  std::ifstream infile(filename.c_str());
  if (!infile) {
    return false;
  }

清空加判断是否有文件输入。

  std::string data_type;
  while (infile.good()) {
    // Read whether the type is a node or a constraint.
    infile >> data_type;
    if (data_type == Pose::name()) {
      if (!ReadVertex(&infile, poses)) {
        return false;
      }
    } else if (data_type == Constraint::name()) {
      ReadConstraint(&infile, constraints);
    } else {
      LOG(ERROR) << "Unknown data type: " << data_type;
      return false;
    }

    // Clear any trailing whitespace from the line.
    infile >> std::ws;
  }

定义string类型data_type它用来判断读入的是VERTEX_SE3:QUAT还是EDGE_SE3:QUAT来执行ReadVertex函数或者ReadConstraint函数。很妙的设计

三、type.h部分

这部分主要是对于一些数据结构的定义优化和对于g2o文件读取的设置。

struct Pose3d {
  Eigen::Vector3d p;
  Eigen::Quaterniond q;

  // The name of the data type in the g2o file format.
  static std::string name() { return "VERTEX_SE3:QUAT"; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

定义Pose3d的结构包含一个vector3d的p和四元数q并且name函数返回VERTEX_SE3:QUAT正好是g2o文件中顶点开头的字符串。

inline std::istream& operator>>(std::istream& input, Pose3d& pose) {
  input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >> pose.q.y() >>
      pose.q.z() >> pose.q.w();
  // Normalize the quaternion to account for precision loss due to
  // serialization.
  pose.q.normalize();
  return input;
}

重写了>>运算符将左侧数据写入pose里面并将q正规化。

typedef std::map<int,
                 Pose3d,
                 std::less<int>,
                 Eigen::aligned_allocator<std::pair<const int, Pose3d>>>
    MapOfPoses;

定义了这么一个别名。

// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
struct Constraint3d {
  int id_begin;
  int id_end;

  // The transformation that represents the pose of the end frame E w.r.t. the
  // begin frame B. In other words, it transforms a vector in the E frame to
  // the B frame.
  Pose3d t_be;

  // The inverse of the covariance matrix for the measurement. The order of the
  // entries are x, y, z, delta orientation.
  Eigen::Matrix<double, 6, 6> information;

  // The name of the data type in the g2o file format.
  static std::string name() { return "EDGE_SE3:QUAT"; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

定义了Constraint3d的结构有id_begin和id_end分别表示序号。

t_be表示由end到begin的变换旋转加平移

information是6*6的信息矩阵

name函数返回EDGE_SE3:QUAT字符串。

inline std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
  Pose3d& t_be = constraint.t_be;
  input >> constraint.id_begin >> constraint.id_end >> t_be;

  for (int i = 0; i < 6 && input.good(); ++i) {
    for (int j = i; j < 6 && input.good(); ++j) {
      input >> constraint.information(i, j);
      if (i != j) {
        constraint.information(j, i) = constraint.information(i, j);
      }
    }
  }
  return input;
}

重写了>>运算符并把相对应的量分别输入注意对于信息矩阵的赋值因为g2o文件的信息矩阵只存了上三角所要要这样写。

typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d>>
    VectorOfConstraints;

另一个别名。

四、pose_graph_3d.cc部分

从main开始看起

  ceres::examples::MapOfPoses poses;
  ceres::examples::VectorOfConstraints constraints;

前面部分都是一些读参这两条是运用了上面type.h里面的两个别名。

poses是一个mapkey为int类型value是Pose3d类型的变量

constraints是一个vector类型存储为Constraint3d类型的变量。

  CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints))
      << "Error reading the file: " << FLAGS_input;

  std::cout << "Number of poses: " << poses.size() << '\n';
  std::cout << "Number of constraints: " << constraints.size() << '\n';

  CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
      << "Error outputting to poses_original.txt";

进入ReadG2oFile函数读取g2o文件并输出一些原始数据。

  ceres::Problem problem;
  ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);

设置problem对象并开始优化的相关操作。

具体看BuildOptimizationProblem函数。

// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
void BuildOptimizationProblem(const VectorOfConstraints& constraints,
                              MapOfPoses* poses,
                              ceres::Problem* problem)

注释的意思是构造通过图限制来构造最小二乘的优化问题。BuildOptimizationProblem函数的第一个参数是constraints表示两点之间的约束的数据结构

第二个是poses代表顶点的数据

第三个就是ceres的problem。

  CHECK(poses != NULL);
  CHECK(problem != NULL);
  if (constraints.empty()) {
    LOG(INFO) << "No constraints, no problem to optimize.";
    return;
  }

判断参数是否为空。

  ceres::LossFunction* loss_function = NULL;
  ceres::LocalParameterization* quaternion_local_parameterization =
      new EigenQuaternionParameterization;

设置loss_function为空指针说明没有设置核函数

设置了ceres关于四元数相关计算变量。

  for (VectorOfConstraints::const_iterator constraints_iter =
           constraints.begin();
       constraints_iter != constraints.end();
       ++constraints_iter) {
    const Constraint3d& constraint = *constraints_iter;

    MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
    CHECK(pose_begin_iter != poses->end())
        << "Pose with ID: " << constraint.id_begin << " not found.";
    MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
    CHECK(pose_end_iter != poses->end())
        << "Pose with ID: " << constraint.id_end << " not found.";

    const Eigen::Matrix<double, 6, 6> sqrt_information =
        constraint.information.llt().matrixL();
    // Ceres will take ownership of the pointer.
    ceres::CostFunction* cost_function =
        PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);

    problem->AddResidualBlock(cost_function,
                              loss_function,
                              pose_begin_iter->second.p.data(),
                              pose_begin_iter->second.q.coeffs().data(),
                              pose_end_iter->second.p.data(),
                              pose_end_iter->second.q.coeffs().data());

    problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
                                 quaternion_local_parameterization);
    problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
                                 quaternion_local_parameterization);
  }

在这个for循环中完成对于边限制数据的读取计算。下面进行详细的分析

  for (VectorOfConstraints::const_iterator constraints_iter =
           constraints.begin();
       constraints_iter != constraints.end();
       ++constraints_iter)

因为是vector容器所以可以使用迭代器。

    const Constraint3d& constraint = *constraints_iter;

    MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
    CHECK(pose_begin_iter != poses->end())
        << "Pose with ID: " << constraint.id_begin << " not found.";
    MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
    CHECK(pose_end_iter != poses->end())
        << "Pose with ID: " << constraint.id_end << " not found.";

取出begin_id和end_id就是数据的前两个数值分别赋值给pose_begin_iter和pose_end_iter。注意它是用find函数在poses里面取。

    const Eigen::Matrix<double, 6, 6> sqrt_information =
        constraint.information.llt().matrixL();

将信息矩阵进行llt分解并赋值给sqrt_information矩阵Eigen的LLT分解实现了Cholesky 分解。

Eigen的LLT分解_火星机器人life的博客-CSDN博客_eigen llt

    // Ceres will take ownership of the pointer.
    ceres::CostFunction* cost_function =
        PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);

通过Create构造函数生成cost_function。

    problem->AddResidualBlock(cost_function,
                              loss_function,
                              pose_begin_iter->second.p.data(),
                              pose_begin_iter->second.q.coeffs().data(),
                              pose_end_iter->second.p.data(),
                              pose_end_iter->second.q.coeffs().data());

这里是proble对象的AddResidualBlock模块

第一个参数cost_function是设定的代价函数

第二个参数loss_function是核函数没有定义

第三个参数pose_begin_iter->second.p.data()是第一个点的位置变量

第四个参数pose_begin_iter->second.q.coeffs().data()是第一个点的旋转变量coeffs()是xyzw的顺序

后面两个参数类似。

    problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
                                 quaternion_local_parameterization);
    problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
                                 quaternion_local_parameterization);

这里的意思是说四元数不符合常规运算的eigen专门为它配置了运算需要在problem配置时加入。

位姿图优化问题有六个未完全约束的自由度。 这通常称为规范自由度。 
您可以对所有节点应用刚体变换优化问题仍将具有完全相同的成本。  
Levenberg-Marquardt 算法具有可缓解此问题的内部阻尼但最好适当地约束规范自由度。 
这可以通过将其中一个姿势设置为常量来实现这样优化器就无法更改它。

大概意思是第一个点固定

  MapOfPoses::iterator pose_start_iter = poses->begin();
  CHECK(pose_start_iter != poses->end()) << "There are no poses.";
  problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
  problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());

取出poses的第一个赋值给迭代器pose_start_iter然后将它的p和q都在problem中设置成恒定值就是上面那段翻译的意思。

再回到main里面来。

  CHECK(ceres::examples::SolveOptimizationProblem(&problem))
      << "The solve was not successful, exiting.";

开始优化就是ceres求导的solve环节。

// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem* problem) {
  CHECK(problem != NULL);

  ceres::Solver::Options options;
  options.max_num_iterations = 200;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  ceres::Solver::Summary summary;
  ceres::Solve(options, problem, &summary);

  std::cout << summary.FullReport() << '\n';

  return summary.IsSolutionUsable();
}

设置solver的options最大迭代次数设置为200次

采用ceres::SPARSE_NORMAL_CHOLESKY稀疏且可夫斯基模式求解

然后开始solve并输入summary的信息最后返回是否成功标识。

  CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
      << "Error outputting to poses_original.txt";

最后将优化后的位姿输出到poses_optimized.txt文件中去。

结束终于算是理解的比较透彻的了。

五、最终运行

在经过我一系列的修改后终于成功的跑起来了。

以下是输出结果

Number of poses: 1661
Number of constraints: 6275

Solver Summary (v 2.0.0-eigen-(3.4.0)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                         3322                     3320
Parameters                              11627                    11620
Effective parameters                     9966                     9960
Residual blocks                          6275                     6275
Residuals                               37650                    37650

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                     3320

Cost:
Initial                          8.362723e+03
Final                            6.341883e-01
Change                           8.362088e+03

Minimizer iterations                       20
Successful steps                           20
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.004724

  Residual only evaluation           0.597028 (20)
  Jacobian & residual evaluation    18.880399 (20)
  Linear solver                      0.425803 (20)
Minimizer                           19.939410

Postprocessor                        0.000370
Total                               19.944509

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 6.104042e-07 <= 1.000000e-06)