Line-Plane intersection && Plane Parameterization

三维空间直线与平面的交点计算与平面方程优化的参数化方法。

1. 线面交点计算

线面交点计算方法有很多种，列出两种，使用何种方法与线面表达的形式有关（形式可以转换，这种关系只涉及便利程度）。

1.1. 方法一

问题描述：有一平面，法向为 \(\mathbf{n}\) ，平面上一点 \(\mathbf{X}_0\)；有一直线，方向为 \(\mathbf{d}\)，直线上一点 \(\mathbf{X}_1\)。求直线与平面的交点。

解：

先判断是否有交点。计算 \(\mathbf{n}\) 与 \(\mathbf{d}\)，如果绝对值小于某值则两者之间的夹角足够接近于 90°（考虑数值精度，不是完全等于 90°），无交点。

否则有交点。

假设交点为 \(\mathbf{X}\)。那么有直线 \(\mathbf{X}_1\mathbf{X}\) 与方向 \(\mathbf{d}\) 平行，直线 \(\mathbf{X}_0\mathbf{X}\) 与方向 \(\mathbf{n}\) 垂直。
写成线性方程如下。

\[\begin{align}(\mathbf{X} - \mathbf{X}_0)^{T} \mathbf{n} &= 0 \\ (\mathbf{X} - \mathbf{X}_1) \times \mathbf{d}&= \mathbf{0} \end{align} \]

整理得。

\[\begin{align}\begin{bmatrix} \mathbf{n}^T \\ [\mathbf{d}_{\times}] \end{bmatrix} \mathbf{X} = \begin{bmatrix} \mathbf{n}^T\mathbf{X}_0 \\ [\mathbf{d}_{\times}]\mathbf{X}_1 \end{bmatrix}\end{align} \]

解之即得（\([\mathbf{d}_{\times}]\) 的秩只有 2，所以以上 4 个方程的后 3 个仅需取 2 个，3 个方程解 3 个未知数能够保证只有 1 个或 0 个解，因为前面已经做了向量方向判断，排除 0 个解的情况，剩下 1 个解的情况）。

1.2. 方法二

考虑到希望优化线与平面方程，需要计算导数，以上交点 \(\mathbf{X}\) 的结果是隐式的，无法计算导数。

问题描述：有一平面，法向为 \(\mathbf{n}\) ，平面上一点 \(\mathbf{X}_0\)；有一直线，方向为 \(\mathbf{d}\)，直线上一点 \(\mathbf{X}_1\)。求直线与平面的交点。

平面方程可以用 \(\mathbf{\pi}\) 表示（\(\mathbf{\pi}\mathbf{X}=AX+BY+CZ+D=0\)）。\(\mathbf{\pi}\) 与 \(\mathbf{n}, \mathbf{X_0}\) 的关系如下。

\[\begin{align}\mathbf{\pi} = \begin{bmatrix} \mathbf{n} \\ -\mathbf{n}^T \mathbf{X}_0\end{bmatrix} = \begin{bmatrix} n_x \\ n_y \\ n_z \\ -n_xX_0 - n_yY_0 - n_zZ_0\end{bmatrix}\end{align} \]

直线方程可以用 Plucker Matrix \(\mathbf{L}\) 表示。\(\mathbf{L}\) 与 \(\mathbf{X}, \mathbf{d}\) 之间的关系如下。（Plucker Matrix 参考 [1] 3.2 Representing and transforming planes, lines and quadrics Page 72）

\[\begin{align} \mathbf{L} = \mathbf{X}_1\mathbf{d}^T - \mathbf{d}\mathbf{X}_1^T \end{align} \]

直线 \(\mathbf{L}\) 与平面 \(\mathbf{\pi}\) 的交点 \(\mathbf{X}\) （齐次）如下。

\[\begin{align}\mathbf{X} = \mathbf{L} \mathbf{\pi}\end{align} \]

2. 导数计算

按照方法二计算导数。

为方便区分齐次与非齐次，令。

\[\begin{align}\mathbf{\pi} = \begin{bmatrix} \mathbf{\Pi} \\ \pi \end{bmatrix}\end{align} \]

整理交点 \(\mathbf{X}\) 方程（符号不容易写，请自行分辨一下齐次与非齐次坐标）。

\[\begin{align} \mathbf{X} &= (\mathbf{X}_1\mathbf{d}^T - \mathbf{d}\mathbf{X}_1^T) \mathbf{\pi} \\ &= \mathbf{X}_1\mathbf{d}^T\mathbf{\pi} - \mathbf{d}\mathbf{X}_1^T \mathbf{\pi} \\ &= (\mathbf{d}^T\mathbf{\pi}\mathbf{I} - \mathbf{d}\mathbf{\pi}^T)\mathbf{X}_1 \\ &= \left( \begin{bmatrix} \mathbf{d}^T & 0 \end{bmatrix} \begin{bmatrix} \mathbf{\Pi} \\ \pi \end{bmatrix} \mathbf{I} - \begin{bmatrix} \mathbf{d} \\ 0 \end{bmatrix} \begin{bmatrix} \mathbf{\Pi}^T & \pi \end{bmatrix} \right) \begin{bmatrix} \mathbf{X}_1 \\ 1 \end{bmatrix} \\ &= \left( \mathbf{d}^T\mathbf{\Pi}\mathbf{I} - \begin{bmatrix} \mathbf{d}\mathbf{\Pi}^T & \mathbf{d}\pi \\ \mathbf{0}^T & 0 \end{bmatrix} \right) \begin{bmatrix} \mathbf{X}_1 \\ 1 \end{bmatrix} \\ &= \begin{bmatrix} \mathbf{d}^T\mathbf{\Pi}\mathbf{I} - \mathbf{d}\mathbf{\Pi}^T & -\mathbf{d}\pi \\ \mathbf{0}^T & \mathbf{d}^T\mathbf{\Pi} \end{bmatrix} \begin{bmatrix} \mathbf{X}_1 \\ 1 \end{bmatrix} \\ &= \begin{bmatrix} (\mathbf{d}^T\mathbf{\Pi}\mathbf{I} - \mathbf{d}\mathbf{\Pi}^T) \mathbf{X}_1 - \pi \mathbf{d} \\ \mathbf{d}^T\mathbf{\Pi} \end{bmatrix} \end{align}\]

非齐次坐标。

\[\begin{align} \mathbf{X} = (\mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \mathbf{d}^T \mathbf{\Pi}}) \mathbf{X}_1 - {\pi \mathbf{d} \over \mathbf{d}^T \mathbf{\Pi}} \end{align}\]

思考直线与平面的自由度的个数，做好计算导数的准备。

在二维空间中直线与点是对偶的，各自有 3 个自由度。在三维空间中平面与点是对偶的，各自有的 3 个自由度。理解三维平面有 3 个自由度可以有两种方式：1. 用截距式表达\({X \over A} + {Y \over B} + {Z \over C} = 1\)，用平面与三轴的交点表达平面方程；2. 用平面的法向量与平面距离原点的距离表达，三维空间方向向量的自由度为 2，距离自由度为 1。

在三维空间中的直线：1. 表达为一个经过的点与方向向量，点的自由度为 3，方向向量自由度为 2，一共 5 个自由度；2. 表达为两个平面的交点，两个平面的自由度为 6，两个平面的 range space 无重合，则两个平面不相交，两个平面的 range space 有 1 维重合，两个平面相交于一条线，直线自由度为 6 - 1 = 5。

以上关于空间中直线自由度的这一段是 bullshit 。

计算导数的时候，直线方程用点 \(\mathbf{X}_1\)、方向 \(\mathbf{d}\) 表达，平面方程用 \(\mathbf{\pi}\) 表达，计算导数就是计算 \(\mathbf{X}\) 对 \(\mathbf{X}_1, \mathbf{d}, \mathbf{\pi}\) 的导数。

先计算两个简单的。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{X}_1} &= \mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \mathbf{d}^T \mathbf{\Pi}} \\ {\partial \mathbf{X} \over \partial \pi} &= - {\mathbf{d} \over \mathbf{d}^T \mathbf{\Pi}} \end{align}\]

计算对 \(\mathbf{d}\) 的导数。

变换 \(\mathbf{X}\) 方程的形式，令 \(\lambda=\mathbf{d}^T \mathbf{\Pi}\)。

\[\begin{align} \mathbf{X} &= (\mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \mathbf{d}^T \mathbf{\Pi}}) \mathbf{X}_1 - {\pi \mathbf{d} \over \mathbf{d}^T \mathbf{\Pi}} \\ &= -{\mathbf{\Pi}^T\mathbf{X}_1 \over \lambda} \mathbf{d} - {\pi \over \lambda} \mathbf{d} + \mathbf{X}_1 \\ &= (-\mathbf{\Pi}^T\mathbf{X}_1 - \pi){\mathbf{d}\over\lambda} + \mathbf{X}_1 \end{align}\]

每个 element 分别计算。

\[\begin{align} {\mathbf{d} \over \lambda} &= \begin{bmatrix} {d_1 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3} \\ {d_2 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3} \\ {d_3 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3} \end{bmatrix} \\ {\partial {\mathbf{d} \over \lambda}\over \partial \mathbf{d}} &= {1 \over \lambda}(\mathbf{I} - {1\over\lambda}\mathbf{d}\mathbf{\Pi}^T) \\ {\partial \mathbf{X} \over \partial \mathbf{d}} &= (-\mathbf{\Pi}^T\mathbf{X}_1 - \pi){1 \over \lambda}(\mathbf{I} - {1\over\lambda}\mathbf{d}\mathbf{\Pi}^T) \end{align}\]

计算对 \(\mathbf{\Pi}\) 的导数。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{\Pi}} &= -{\partial \mathbf{\Pi}^T\mathbf{X}_1{\mathbf{d} \over \lambda}\over \partial \mathbf{\Pi}} - {\partial {\pi \mathbf{d} \over \mathbf{d}^T\mathbf{\Pi}}\over \partial \mathbf{\Pi}} \end{align}\]

后一项为。

\[\begin{align} {\partial {\pi \mathbf{d} \over \mathbf{d}^T\mathbf{\Pi}}\over \partial \mathbf{\Pi}}&=-{\pi\over \lambda^2}\mathbf{d}\mathbf{d}^T \end{align}\]

前一项每个 element 分别计算。

\[\begin{align} \mathbf{\Pi}^T\mathbf{X}_1{\mathbf{d} \over \lambda} &= {X_1\Pi_1 + Y_1\Pi_2 + Z_1\Pi_3 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3}\mathbf{d} \end{align}\]

令 \(\eta=\mathbf{\Pi}^T\mathbf{X}_1\)。

\[\begin{align} {\partial {\eta \over \lambda}\over \partial \Pi_1} &= {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) \\ {\partial {\eta \over \lambda}\over \partial \Pi_2} &= {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) \\ {\partial {\eta \over \lambda}\over \partial \Pi_3} &= {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) \\ \end{align}\]

\[\begin{align} {\partial \mathbf{\Pi}^T\mathbf{X}_1{\mathbf{d} \over \lambda}\over \partial \mathbf{\Pi}} &= \begin{bmatrix} {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_1 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_1 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_1 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_2 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_2 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_2 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_3 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_3 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_3 \end{bmatrix} \end{align}\]

所以。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{\Pi}} &= -\begin{bmatrix} {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_1 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_1 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_1 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_2 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_2 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_2 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_3 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_3 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_3 \end{bmatrix} + {\pi\over \lambda^2}\mathbf{d}\mathbf{d}^T \end{align}\]

综合以上。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{X}_1} &= \mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \lambda} \\ {\partial \mathbf{X} \over \partial \mathbf{d}} &= (-\eta - \pi){1 \over \lambda}(\mathbf{I} - {1\over\lambda}\text{diag}(d_1\Pi_1, d_2\Pi_2, d_3\Pi_3)) \\ {\partial \mathbf{X} \over \partial \mathbf{\Pi}} &= -\begin{bmatrix} {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_1 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_1 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_1 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_2 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_2 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_2 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_3 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_3 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_3 \end{bmatrix} + {\pi\over \lambda^2}\mathbf{d}\mathbf{d}^T \\ {\partial \mathbf{X} \over \partial \pi} &= - {\mathbf{d} \over \lambda} \\ \end{align}\]

3. 参数化

以上方向向量 \(\mathbf{d}\) 用 3 个数字表达，但是仅有 2 个自由度。所以需要做参数化，将对 3 维的导数转换为对 2 维的导数，随时计算一个切面即可。

平面方程 \(\mathbf{\pi}\) 也需要做参数化。注意到平面方程的最后一个数字是 \(-\mathbf{n}^T \mathbf{X}_0\)，即原点到平面距离，如果能确认平面与原点的距离足够大，不会接近 0，可以直接将最后一个数字设置为 1，其他数字自由优化。

不对平面做限制。平面方程 \(\mathbf{\pi}\) 用 4 个数字表达，实际有 3 个自由度。具体这 3 个自由度是什么，不重要。重要的是有 3 个自由度。如果能套用三维方向向量的参数化方式，那是极好的，但是三维向量的参数化方式涉及到计算叉乘，叉乘在四维空间中不存在，参考 [2]。从 4 维到 3 维，与四元数的维度一致，可以参考四元数的参数化方式，参考 [3] (18)。四元数的 global to local 的 4x3 矩阵的每一列都是四元数的零空间，也是平面方程的零空间。所以找到了 global to local 方法。

对于 plus 方法，即，计算得到的零空间三个方向的增量，将这三个增量乘以三个方向加到平面方程上，最后对平面方程做归一化，保证平面方程的模为 1。

4. 线优化参数化示例代码

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <random>

using namespace std;
using namespace ceres;

class CostFunctor {
public:
  CostFunctor(const Eigen::Vector3d &_X0) : X0_{_X0} {}

  template <typename T> bool operator()(const T *_line, T *e) const {
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> X(_line);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> d(_line + 3);
    const Eigen::Matrix<T, 3, 1> X0 = X0_.template cast<T>();

    Eigen::Map<Eigen::Matrix<T, 3, 1>> res(e);
    res = (Eigen::Matrix<T, 3, 3>::Identity() - d * d.transpose()) * (X0 - X);

    return true;
  }

  const Eigen::Vector3d X0_;
};

class MyLineParameterization : public ceres::LocalParameterization {
public:
  virtual ~MyLineParameterization() {}
  bool Plus(const double *x, const double *delta, double *x_plus_delta) const {
    Eigen::Map<const Eigen::Matrix<double, 3, 1>> d(x + 3);
    Eigen::Matrix<double, 3, 1> d0 =
        d.cross(Eigen::Matrix<double, 3, 1>(1., 0., 0.));
    if (d0.norm() < 0.05) {
      d0 = d.cross(Eigen::Matrix<double, 3, 1>(0., 1., 0.));
    }
    d0.normalize();
    Eigen::Matrix<double, 3, 1> d1 = d.cross(d0);
    d1.normalize();

    Eigen::Map<const Eigen::Matrix<double, 3, 1>> X(x);

    Eigen::Map<Eigen::Matrix<double, 3, 1>> X_prime(x_plus_delta);
    Eigen::Map<Eigen::Matrix<double, 3, 1>> d_prime(x_plus_delta + 3);
    X_prime = X + d0 * delta[0] + d1 * delta[1];
    d_prime = d + d0 * delta[2] + d1 * delta[3];
    d_prime.normalize();

    return true;
  }

  bool ComputeJacobian(const double *x, double *jacobian) const {
    Eigen::Map<const Eigen::Matrix<double, 3, 1>> d(x + 3);
    Eigen::Matrix<double, 3, 1> d0 =
        d.cross(Eigen::Matrix<double, 3, 1>(1., 0., 0.));
    if (d0.norm() < 0.05) {
      d0 = d.cross(Eigen::Matrix<double, 3, 1>(0., 1., 0.));
    }
    d0.normalize();
    Eigen::Matrix<double, 3, 1> d1 = d.cross(d0);
    d1.normalize();
    Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>> J(jacobian);
    J.setZero();
    J.template block<3, 1>(0, 0) = d0;
    J.template block<3, 1>(0, 1) = d1;
    J.template block<3, 1>(3, 2) = d0;
    J.template block<3, 1>(3, 3) = d1;
    return true;
  }

  int GlobalSize() const override { return 6; }
  int LocalSize() const override { return 4; }
};

double Evaluate(const std::vector<CostFunction *> &cost_functions,
                Eigen::Matrix<double, 6, 1> &line_opt, double *J_data,
                double *e_data,
                Eigen::Matrix<double, 6, 4, Eigen::RowMajor> &global_to_local) {
  const int N = cost_functions.size();
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>> e(e_data, 3 * N, 1);
  if (J_data) {
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 4, Eigen::RowMajor>> J(
        J_data, 3 * N, 4);
    for (int i = 0; i < N; ++i) {
      Eigen::Matrix<double, 3, 6, Eigen::RowMajor> J_global;
      double **parameters = new double *[1];
      parameters[0] = line_opt.data();
      Eigen::Matrix<double, 3, 1> residuals;
      double **jacobians = new double *[1];
      jacobians[0] = J_global.data();
      bool ret =
          cost_functions[i]->Evaluate(parameters, residuals.data(), jacobians);
      Eigen::Matrix<double, 3, 4, Eigen::RowMajor> J_local =
          J_global * global_to_local;
      J.block(i * 3, 0, 3, 4) = J_local;
      e.block(i * 3, 0, 3, 1) = residuals;
      delete[] parameters;
      delete[] jacobians;
    }
  } else {
    for (int i = 0; i < N; ++i) {
      double **parameters = new double *[1];
      parameters[0] = line_opt.data();
      Eigen::Matrix<double, 3, 1> residuals;
      bool ret =
          cost_functions[i]->Evaluate(parameters, residuals.data(), nullptr);
      e.block(i * 3, 0, 3, 1) = residuals;
      delete[] parameters;
    }
  }
  return e.norm();
}

int main() {
  std::srand(std::time(NULL));

  double range = 1.;

  Eigen::Vector3d X(rand() / double(RAND_MAX) * 2 * range + (-range),
                    rand() / double(RAND_MAX) * 2 * range + (-range),
                    rand() / double(RAND_MAX) * 2 * range + (-range));
  Eigen::Vector3d d(rand() / double(RAND_MAX) * 2 * range + (-range),
                    rand() / double(RAND_MAX) * 2 * range + (-range),
                    rand() / double(RAND_MAX) * 2 * range + (-range));
  d.normalize();

  Eigen::Matrix<double, 6, 1> line;
  line << X[0], X[1], X[2], d[0], d[1], d[2];

  cout << "ground truth line: " << line.transpose() << endl;

  const int N = 200;

  default_random_engine generator;
  normal_distribution<double> distribution(0.0, 0.3);

  std::vector<Eigen::Vector3d> points;
  points.reserve(N);

  range = 4.;
  Eigen::Vector3d X_opt;
  X_opt.setZero();
  for (int i = 0; i < N; ++i) {
    const double t = rand() / double(RAND_MAX) * 2 * range + (-range);
    Eigen::Vector3d X0 = X + t * d;
    X0[0] += distribution(generator);
    X0[1] += distribution(generator);
    X0[2] += distribution(generator);
    points.push_back(X0);
    X_opt += X0;
  }
  X_opt /= N;

  // Build the problem.
  Problem problem;

  MyLineParameterization *parameterization = new MyLineParameterization();
  Eigen::Matrix<double, 6, 1> line_opt;
  line_opt << 1., 0., 0., 1., 0., 0.;
  problem.AddParameterBlock(line_opt.data(), 6, parameterization);

  std::vector<CostFunction *> cost_functions;
  cost_functions.reserve(N);

  for (Eigen::Vector3d &point : points) {
    CostFunction *cost_function =
        new AutoDiffCostFunction<CostFunctor, 3, 6>(new CostFunctor(point));
    problem.AddResidualBlock(cost_function, nullptr, line_opt.data());
    cost_functions.push_back(cost_function);
  }

  /// LM algorithm.
  double lambda = 1e-3;
  double total_cost = 0;
  Eigen::LLT<Eigen::Matrix<double, 4, 4>> llt;
  Eigen::Matrix<double, 6, 4, Eigen::RowMajor> global_to_local;
  Eigen::Matrix<double, Eigen::Dynamic, 4, Eigen::RowMajor> J;
  J.resize(3 * N, 4);
  Eigen::Matrix<double, Eigen::Dynamic, 1> e;
  e.resize(3 * N, 1);
  for (int iter = 0; iter < 20; ++iter) {
    parameterization->ComputeJacobian(line_opt.data(), global_to_local.data());
    double total_cost =
        Evaluate(cost_functions, line_opt, J.data(), e.data(), global_to_local);
    Eigen::Matrix<double, 4, 4> JTJ;
    JTJ = J.transpose() * J;
    Eigen::Matrix<double, 4, 1> b;
    b = -J.transpose() * e;
    Eigen::Matrix<double, 4, 4> A;
    A = JTJ + (1. + lambda) *
                  Eigen::Matrix<double, 4, 4>(JTJ.diagonal().asDiagonal());
    llt.compute(A);
    Eigen::Matrix<double, 4, 1> delta_l = llt.solve(b);
    Eigen::Matrix<double, 6, 1> line_opt_prime;
    parameterization->Plus(line_opt.data(), delta_l.data(),
                           line_opt_prime.data());
    double new_total_cost = Evaluate(cost_functions, line_opt_prime, nullptr,
                                     e.data(), global_to_local);
    if (new_total_cost < total_cost && lambda > 1e-5) {
      lambda *= 0.1;
    }
    while (new_total_cost >= total_cost && lambda < 1.) {
      lambda *= 10.;
      Eigen::Matrix<double, 4, 4> A;
      A = JTJ + (1. + lambda) *
                    Eigen::Matrix<double, 4, 4>(JTJ.diagonal().asDiagonal());
      llt.compute(A);
      Eigen::Matrix<double, 4, 1> delta_l = llt.solve(b);
      Eigen::Matrix<double, 6, 1> line_opt_prime;
      parameterization->Plus(line_opt.data(), delta_l.data(),
                             line_opt_prime.data());
      new_total_cost = Evaluate(cost_functions, line_opt_prime, nullptr,
                                e.data(), global_to_local);
    }
    if (lambda >= 1.) {
      cout << "optimization failed" << endl;
      break;
    }
    double delta_parameter = (line_opt - line_opt_prime).norm();
    double delta_cost = total_cost - new_total_cost;
    if (delta_parameter < 1e-4 || delta_cost < 1e-4)
      break;
    line_opt = line_opt_prime;
    cout << "iter: " << iter << "\ttotal_cost: " << total_cost
         << "\tnew_total_cost: " << new_total_cost
         << "\tdelta_total_cost: " << delta_cost << "\tlambda: " << lambda
         << endl;
  }
  // Solver::Options options;
  // options.linear_solver_type = ceres::DENSE_QR;
  // options.minimizer_progress_to_stdout = true;
  // Solver::Summary summary;
  // Solve(options, &problem, &summary);

  // cout << summary.BriefReport() << "\n";
  cout << "ground truth line: " << line.transpose() << endl;
  cout << "optimized line:    " << line_opt.transpose() << endl;
  cout << "direction dot:     "
       << line_opt.template block<3, 1>(3, 0).dot(
              line.template block<3, 1>(3, 0))
       << endl;
  /// X_opt on line?
  double dist =
      ((Eigen::Matrix3d::Identity() -
        line.template block<3, 1>(3, 0) *
            line.template block<3, 1>(3, 0).transpose()) *
       (line_opt.template block<3, 1>(0, 0) - line.template block<3, 1>(0, 0)))
          .norm();
  cout << "optimized X to gt line dist: " << dist << endl;

  return 0;
}

Reference

[1] Hartley, Richard, and Andrew Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2003.

[2] Do four dimensional vectors have a cross product property? [duplicate] https://math.stackexchange.com/questions/720813/do-four-dimensional-vectors-have-a-cross-product-property.

[3] Sola, Joan. "Quaternion kinematics for the error-state KF." Laboratoire dAnalyse et dArchitecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep (2012).