pl_vio线特征·part I

0.引言

• pl_vio线特征·part II

• PL-VIO，本文关注线段。

• 论文链接

• SLAM中线特征的参数化和求导

• SLAM线特征学习（1）——基本的线特征表示与优化推导

• SLAM线特征学习（2）——线特征初始化和最小表示

• SLAM中线特征的参数化表示方法/重投影/初始化方法

1.LineFeatureTracker核心逻辑解读

• 数据结构定义：

• keylsd：提取的线段结果（经过长度筛选的，太短的不要）

• keylbd_descr：对应线段描述子结果（经过长度筛选的，太短的不要）

• 新进的img提取线特征：

  • 如果是第一帧img：forwframe_->lineID.push_back(allfeature_cnt++)；（allfeature_cnt是整个系统全局的线特征计数）,因此lineID是全局的ID，整个系统中线特征具有唯一的ID。update:后面更新的是没匹配上的。
  • 如果不是第一帧img，则暂时赋值为-1，但forwframe_->lineID数组的size的大小是和keylsd相同的；
    • 匹配后将-1进行更改：forwframe_->lineID[mt.queryIdx] = curframe_->lineID[mt.trainIdx];
      • Key = 当前帧线段的ID
      • Value = 前一帧线段的lineID[mt.trainIdx]
    • 判断当前帧水平，垂直方向线段数量是否分别足够35条，不够的补上。

匹配的结果，对应关系，其实就是存储的全局ID信息。

代码注释，可惜另一个函数的关键内容lineDetector没放出来，这个函数的vecLine只在最后做了一个转换。frame_id没维护。

字数超限，删除代码，代码是这个函数。

最后forwframe_->vecLine属性，赋值了三个

l.StartPt = lsd.getStartPoint();
l.EndPt = lsd.getEndPoint();
l.length = lsd.lineLength;

ROS发布的信息

• feature_lines->points ： 线段起点
• feature_lines->channels.push_back(id_of_line); ：与上一帧的匹配关系
• feature_lines->channels.push_back(u_of_endpoint);：线段终点 u 坐标
• feature_lines->channels.push_back(v_of_endpoint);：线段终点 v 坐标

在ROS中，sensor_msgs::PointCloud是一个用于表示3D点云的消息类型。它的数据结构如下：

• std_msgs/Header header：消息头，包含时间戳和坐标帧信息。
• geometry_msgs/Point32[] points：点云中的点，每个点包含x、y和z坐标。
• std_msgs/ChannelFloat32[] channels：每个通道包含一个名字和一组与点对应的浮点数。这可以用来存储与每个点相关的额外数据，如颜色、强度等，这里存储的信息如上所述。

2.estimator_node中线段的处理

2.1.订阅信息解压

map<int, vector<pair<int, Vector4d>>> lines;
for (unsigned int i = 0; i < line_msg->points.size(); i++) {
  int v = line_msg->channels[0].values[i] + 0.5;
  // std::cout<< "receive id: " << v / NUM_OF_CAM << "\n";
  int feature_id = v / NUM_OF_CAM;
  int camera_id =
      v % NUM_OF_CAM;  // 被几号相机观测到的，如果是单目，camera_id = 0
  double x_startpoint = line_msg->points[i].x;
  double y_startpoint = line_msg->points[i].y;
  double x_endpoint = line_msg->channels[1].values[i];
  double y_endpoint = line_msg->channels[2].values[i];
  //                ROS_ASSERT(z == 1);
  lines[feature_id].emplace_back(
      camera_id,
      Vector4d(x_startpoint, y_startpoint, x_endpoint, y_endpoint));
}

map<int, vector<pair<int, Vector4d>>> lines; 其实这里的vetor大小始终是1

• key : 与上一帧线特征的对应关系
• value : camera_id + 线段起点 + 线段终点

2.2.线特征管理

现根据之前匹配的对应关系构造出lineFeaturePerId信息：

// 根据frame进行维护，这里是最初读取的数据
class lineFeaturePerFrame {
 public:
  lineFeaturePerFrame(const Vector4d &line) { lineobs = line; }
  lineFeaturePerFrame(const Vector8d &line) {
    lineobs = line.head<4>();
    lineobs_R = line.tail<4>();
  }
  Vector4d lineobs;  // 每一帧上的观测
  Vector4d lineobs_R;
  double z;
  bool is_used;
  double parallax;
  MatrixXd A;
  VectorXd b;
  double dep_gradient;
};

// 根据id进行维护，该id的线段可能被多帧观测到
class lineFeaturePerId {
 public:
  const int feature_id;   //线特征的全局ID,全局唯一
  int start_frame;
  //  linefeature_per_frame 是个向量容器，存着这个特征在每一帧上的观测量。
  //                    如：feature_per_frame[0]，存的是ft在start_frame上的观测值;
  //                    feature_per_frame[1]存的是start_frame+1上的观测
  // 同一id在不同帧上的观测，统一放到一起，后续会进行三角化
  vector<lineFeaturePerFrame> linefeature_per_frame;

  int used_num;
  bool is_outlier;
  bool is_margin;
  bool is_triangulation;
  Vector6d line_plucker;

  Vector4d obs_init;
  Vector4d obs_j;
  Vector6d line_plk_init;  // used to debug
  Vector3d ptw1;           // used to debug
  Vector3d ptw2;           // used to debug
  Eigen::Vector3d tj_;     // tij
  Eigen::Matrix3d Rj_;
  Eigen::Vector3d ti_;  // tij
  Eigen::Matrix3d Ri_;
  int removed_cnt;
  int all_obs_cnt;   // 这个id的线特征总共被观测了多少次

  int solve_flag;  // 0 haven't solve yet; 1 solve succ; 2 solve fail;
  int endFrame();
};

构造的逻辑，理解了之前的对应关系是怎么存储的这里就很好理解了：

 // 遍历同一帧上的线特征
 for (auto &id_line : lines) {
   // 使用起点终点初始化
   lineFeaturePerFrame f_per_fra(id_line.second[0].second);  // 观测
   // 这是与上一帧的匹配关系，也可以认为是全局线特征id
   int feature_id = id_line.first;  
   // cout << "line id: "<< feature_id << "\n";
   // 在feature里找id号为feature_id的特征
   auto it = find_if(linefeature.begin(), linefeature.end(),
                     [feature_id](const lineFeaturePerId &it) {
                       return it.feature_id == feature_id;
                     });
   // 如果之前没存这个特征，说明是新的
   if (it == linefeature.end()) {
     linefeature.push_back(lineFeaturePerId(feature_id, frame_count));
     linefeature.back().linefeature_per_frame.push_back(f_per_fra);
   } else if (it->feature_id == feature_id) {
     it->linefeature_per_frame.push_back(f_per_fra);
     it->all_obs_cnt++;
   }
 }

3.线段的表示与三角化

• 这部分来自论文原文。
• 初始化和进行空间变换的时候使用Plücker坐标，
• 在优化的时候使用正交表示。
• 坐标表示
  • 普吕克坐标：$\mathcal{L}={\left({\mathbf{n}}^{?},{\mathbf{v}}^{?}\right)}^{?}\in {\mathbb{R}}^6$
  • 正交表示：$(\mathbf{U},\mathbf{W})\in SO(3)\times SO(2)$

3.1.普吕克线坐标

在图 $2\mathrm{a}$ 中, 普吕克坐标中的 $3\mathrm{D}$ 空间线 $\mathcal{L}$ 由 $\mathcal{L}={\left({\mathbf{n}}^{?},{\mathrm{d}}^{?}\right)}^{?}\in {\mathbb{R}}^6$ 表示, 其中 $\mathrm{d}\in {\mathbb{R}}^3$ 是线方向向量, $\mathrm{n}\in {\mathbb{R}}^3$ 是由直线和坐标原点确定的平面法向量。Plücker 坐标被过度参数化，因为向量 $\mathrm{n}$ 和 $\mathrm{d}$ 之间存在隐式约束，即 ${\mathrm{n}}^{?}\mathrm{d}=0$ 。因此, Plücker坐标不能直接用于无约束优化。然而, 使用由法线向量和方向向量表示的 3D 线, 可以简单地从两个几何视图执行三角测量, 并且也可以方便地对线几何变换进行建模。

对于线几何变换, 给定从世界坐标系 $\mathrm{w}$ 到相机坐标系 $\mathrm{c}$ 的变换矩阵 ${\mathbf{T}}_{cw}=\left[\begin{array}{cc}{\mathbf{R}}_{cw}& {\mathbf{P}}_{cw}\\ 0& 1\end{array}\right]$, 我们可以将线的 Plücker 坐标变换为 [30]
$${\mathcal{L}}^c=\left[\begin{array}{c}{\mathbf{n}}^c\\ {\mathbf{d}}^c\end{array}\right]=J_{cw}{\mathcal{L}}_w=\left[\begin{array}{cc}{\mathbf{R}}_{cw}& {\left[{\mathbf{p}}_{cw}\right]}_x{\mathbf{R}}_{cw}\\ 0& {\mathbf{R}}_{cw}\end{array}\right]{\mathcal{L}}^w$$

其中 $[?{]}_{\times }$是三维向量的斜对称矩阵， $J_{cw}$ 是用于将线从帧 $w$ 变换到帧 $\mathrm{c}$ 的变换矩阵。

当在两个不同的摄像机视图中观察到新的线地标时，可以轻松计算出普吕克坐标。如图 $2\mathrm{b}$ 所示，3D 线 $\mathcal{L}$ 由摄像机 $c_1$ 和 $c_2$ 捕获为 ${\mathrm{z}}_L^{c_1}$ 和 ${\mathrm{z}}_l^{c_2}$。归一化图像平面中的线段 ${\mathrm{z}}_L^{c_1}$ 可以由两个端点 ${\mathrm{s}}^{c_1}={\left[u_s,v_s,1\right]}^T$ 和 ${\mathrm{e}}^{c_1}={\left[u_e,v_e,1\right]}^{?}$ 表示。三个不共线的点，包括线段的两个端点和坐标原点 $\mathrm{C}={\left[x_0,y_0,z_0\right]}^{?}$, 确定 $3\mathrm{D}$ 空间中的平面 $\pi ={\left[{\pi }_x,{\pi }_y,{\pi }_z,{\pi }_w\right]}^{?}$ :
$${\pi }_x\left(x?x_0\right)+{\pi }_y\left(y?y_0\right)+{\pi }_z\left(z?z_0\right)=0$$
这里
$$\left[\begin{array}{c}{\pi }_x\\ {\pi }_y\\ {\pi }_z\end{array}\right]={\left[{\mathbf{s}}^{c_1}\right]}_{\times }{\mathrm{e}}^{c_1},{\pi }_w={\pi }_x{x}_0+{\pi }_y{y}_0+{\pi }_z{z}_0$$

• 这里对偶矩阵的内容可以看《多视图几何》

给定相机坐标系 $c_1$ 中的两个平面 ${\pi }_1$ 和 ${\pi }_2$, 对偶普吕克矩阵 ${\mathrm{L}}^{?}$ 可以通过以下方式计算
$${\mathbf{L}}^{?}=\left[\begin{array}{cc}[\mathbf{d}{]}_{\mathbf{x}}& \mathbf{n}\\ ?{\mathbf{n}}^{?}& 0\end{array}\right]={\pi }_1{\pi }_2^{?}?{\pi }_2{\pi }_1^{?}\in {\mathbb{R}}^{4\times 4}$$

相机坐标系 $c_1$ 中的 Plücker 坐标 $\mathcal{L}=({\mathbf{n}}^T,{\mathbf{d}}^T{)}^T$很容易从对偶 Plücker 矩阵 $L^{?}$ 中提取。可以看出, $\mathbf{n}$ 和 $\mathrm{d}$ 不需要是单位向量。

主体逻辑在void FeatureManager::triangulateLine(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]) 中：

• 根据线段起点终点相机原点求平面：

欧式空间3D平面有3个自由度，对其表示方式主要有两种：Hesse形式、球坐标。

• Hesse形式

$$\pi ={\left({\mathbf{n}}^{?},d\right)}^{?}\in {\mathbb{R}}^4$$

• 球坐标

$$ \tau=(\theta, \phi, d)^{\top}=q(\pi)=\left(\theta=\arctan \frac{n_y}{n_x}, \phi=\arcsin n_z, d\right)^{\top} $$

一般优化中采用球坐标的表示方式

/*
 三点确定一个平面 a(x-x0)+b(y-y0)+c(z-z0)=0  --> ax + by + cz + d = 0   d = -(ax0 + by0 + cz0)
 平面通过点（x0,y0,z0）以及垂直于平面的法线（a,b,c）来得到
 (a,b,c)^T = vector(AO) cross vector(BO)
 d = O.dot(cross(AO,BO))
 */
Vector4d pi_from_ppp(Vector3d x1, Vector3d x2, Vector3d x3) {
    Vector4d pi;
    pi << ( x1 - x3 ).cross( x2 - x3 ), - x3.dot( x1.cross( x2 ) ); // d = - x3.dot( (x1-x3).cross( x2-x3 ) ) = - x3.dot( x1.cross( x2 ) )
    return pi;
}

如果两平面夹角小于 15 度，不三角化。两个平面求交线，即是进行三角化：

// 两平面相交得到直线的plucker 坐标
Vector6d pipi_plk( Vector4d pi1, Vector4d pi2){
    Vector6d plk;
    Matrix4d dp = pi1 * pi2.transpose() - pi2 * pi1.transpose();

    plk << dp(0,3), dp(1,3), dp(2,3), - dp(1,2), dp(0,2), - dp(0,1);
    return plk;
}

这里得到的结果实际上是归一化平面坐标系上的三角化结果。

 Vector6d plk = pipi_plk(pii, pij);
 Vector3d n = plk.head(3);
 Vector3d v = plk.tail(3);

// ...plk坐标的矩阵形式
 Vector3d pc, nc, vc;
 nc = it_per_id.line_plucker.head(3);
 vc = it_per_id.line_plucker.tail(3);

 Matrix4d Lc;
 Lc << skew_symmetric(nc), vc, -vc.transpose(), 0;

• 完整三角化逻辑在这个函数里面。

3.2.正交表示

由于3D空间的直线只有4个自由度，使用Plücker坐标这种过参数化的表示形式在优化的时候不方便，所以我们引入四个参数的正交表示 $(\mathbf{U},\mathbf{W})\in SO(3)\times SO(2)$ 。Plücker坐标和正交表示之间可以很方便的互相转换，我们之后会分别介绍如何从Plücker坐标到正交表示和正交表示怎么变换成Plücker坐标。首先我们需要知道了直线的Plücker坐标 $\mathcal{L}=({\mathbf{n}}^{?},{\mathbf{d}}^{?}{)}^{?}$ ，然后对 $\mathcal{L}$ 进行QR分解，得到：

$\left[\begin{array}{ll}\mathbf{n}& \mathbf{d}\end{array}\right]=\left[\begin{array}{ccc}\frac{\mathbf{n}}{\Vert \mathbf{n}\Vert }& \frac{\mathbf{d}}{\Vert \mathbf{d}\Vert }& \frac{\mathbf{n}\times \mathbf{d}}{\Vert \mathbf{n}\times \mathbf{d}\Vert }\end{array}\right]\left[\begin{array}{cc}\Vert \mathbf{n}\Vert & 0\\ 0& \Vert \mathbf{d}\Vert \\ 0& 0\end{array}\right]=\mathrm{UW}$

分解得到的第一项是正交矩阵$\mathbf{U}$ ，是一个旋转矩阵。所表示的是相机坐标系到直线坐标系的旋转。其中直线坐标系的定义如下：用直线的方向向量以及直线和光心组成平面的法向量作为坐标的两个轴，再用他们叉乘得到的向量作为第三个轴，所以

$$\mathbf{U}=\mathbf{R}(\mathbf{\psi })=\left[\begin{array}{ccc}\frac{\mathbf{n}}{||\mathbf{n}||}& \frac{\mathbf{d}}{||\mathbf{d}||}& \frac{\mathbf{n}\times \mathbf{d}}{||\mathbf{n}\times \mathbf{d}||}\end{array}\right]$$

其中 $\mathbf{\psi }={\left[{\psi }_1,{\psi }_2,{\psi }_3\right]}^{?}$ 代表的是相机坐标系到直线坐标系在 $x$ ， $y$ 和 $z$ 轴的旋转角。

到此我们得到了正交表示的第一项，第二项需要做一些小变换。由于将 （$||\mathbf{n}||,||\mathbf{d}||$） 结合之后只有一个自由度($\mathrm{W}$的秩为2），所以我们可以用三角函数矩阵参数化：

$$\begin{array}{c}\mathbf{W}=\left[\begin{array}{cc}{w}_1& ?w_2\\ w_2& w_1\end{array}\right]=\left[\begin{array}{cc}\cos (?)& ?\sin (?)\\ \sin (?)& \cos (?)\end{array}\right]\\ =\frac{1}{\sqrt{\left(\Vert \mathbf{n}{\Vert }^2+\Vert \mathbf{d}{\Vert }^2\right)}}\left[\begin{array}{cc}\Vert \mathbf{n}\Vert & ?\Vert \mathbf{d}\Vert \\ \Vert \mathbf{d}\Vert & \Vert \mathbf{n}\Vert \end{array}\right]\end{array}$$

$$\begin{array}{c}W=\left[\begin{array}{cc}\frac{1}{\sqrt{1+d^2}}& \frac{d}{\sqrt{1+d^2}}\\ \frac{?d}{\sqrt{1+d^2}}& \frac{1}{\sqrt{1+d^2}}\end{array}\right]=\left[\begin{array}{cc}\frac{\Vert a\Vert }{\sqrt{\Vert a{\Vert }^2+\Vert b{\Vert }^2}}& \frac{\Vert b\Vert }{\sqrt{\Vert a{\Vert }^2+\Vert b{\Vert }^2}}\\ ?\frac{\Vert b\Vert }{\sqrt{\Vert a{\Vert }^2+\Vert b{\Vert }^2}}& \frac{\Vert a\Vert }{\sqrt{\mid a\Vert {}^2+\Vert b{\Vert }^2}}\end{array}\right]=\left[\begin{array}{cc}\cos (?)& ?\sin (?)\\ \sin (?)& \cos (?)\end{array}\right]\\ =\left[\begin{array}{cc}w1& ?w2\\ w2& w1\end{array}\right]\in \mathbf{SO}(2)\end{array}$$
当对 $?$ 进行更新了之后，可以通过 $\cos (?)/\sin (?)$ 的公式来得到直线到原点的距离。

上式中的 $?$ 是旋转角。由于坐标原点（相机光心）到3D直线的距离是 $d=\frac{||\mathbf{n}||}{||\mathbf{d}||}$ ，所以 $\mathbf{W}$ 包含了距离信息 $d$ 的。根据 $\mathbf{U}$ 和 $\mathbf{W}$ 的定义可以看出，4个自由度包括旋转的3个自由度和距离的一个自由度。在优化的时候，我们使用 $\mathcal{O}=[\psi ,?{]}^{?}$ 作为空间直线更新的最小表示。

正交表示到Plücker坐标之间的变换可以通过下面的方式计算出来：

$${\mathcal{L}}^{\prime }={\left[w_1{\mathbf{u}}_1^T,w_2{\mathbf{u}}_2^T\right]}^T=\frac{1}{\sqrt{\left(\Vert \mathbf{n}{\Vert }^2+\Vert \mathbf{d}{\Vert }^2\right)}}\mathcal{L}$$

$$\underset{\text{Plucker?表示?}}{\underbrace{{\mathcal{L}}_{6\times 1}^{\prime }}}=\sqrt{\Vert \mathbf{n}{\Vert }^2+\Vert \mathbf{d}{\Vert }^2}\underset{\text{正交表示?}}{\underbrace{\left[\begin{array}{c}w1{\mathbf{u}}_1^T\\ w2{\mathbf{u}}_2^T\end{array}\right]}}$$

其中 ${\mathbf{u}}_i$ 是矩阵 $\mathbf{U}$ 的第 $i$ 列、 $w_1=\cos (?)$ 和 $w_2=\sin (?)$ 。 ${\mathcal{L}}^{\prime }$ 和 $\mathcal{L}$ 之间存在比例因子，但它们代表相同的3D空间线。

---

• plk转正交表示

  Vector4d plk_to_orth(Vector6d plk) {
    Vector4d orth;
    Vector3d n = plk.head(3);
    Vector3d v = plk.tail(3);
  
    Vector3d u1 = n / n.norm();
    Vector3d u2 = v / v.norm();
    Vector3d u3 = u1.cross(u2);
  
    // todo:: use SO3
    orth[0] = atan2(u2(2), u3(2));
    orth[1] = asin(-u1(2));
    orth[2] = atan2(u1(1), u1(0));
  
    Vector2d w(n.norm(), v.norm());
    w = w / w.norm();
    orth[3] = asin(w(1));
  
    return orth;
  }
  

• 正交表示转plk

  Vector6d orth_to_plk(Vector4d orth) {
    Vector6d plk;
  
    Vector3d theta = orth.head(3);
    double phi = orth[3];
  
    double s1 = sin(theta[0]);
    double c1 = cos(theta[0]);
    double s2 = sin(theta[1]);
    double c2 = cos(theta[1]);
    double s3 = sin(theta[2]);
    double c3 = cos(theta[2]);
  
    Matrix3d R;
    R << c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3, c2 * s3,
        s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3, -s2, s1 * c2, c1 * c2;
  
    double w1 = cos(phi);
    double w2 = sin(phi);
    double d = w1 / w2;  // 原点到直线的距离
  
    Vector3d u1 = R.col(0);
    Vector3d u2 = R.col(1);
  
    Vector3d n = w1 * u1;
    Vector3d v = w2 * u2;
  
    plk.head(3) = n;
    plk.tail(3) = v;
  
    // Vector3d Q = -R.col(2) * d;
    // plk.head(3) = Q.cross(v);
    // plk.tail(3) = v;
  
    return plk;
  }
  

优化的时候使用正交表示。这部分操作位于line_geometry.h/cpp

4.线段残差对位姿的导数

• 这一小节理论部分来自这里,当然也是来自原论文。

4.1.直线的观测模型和误差

图2 空间直线投影到像素平面
要想知道线特征的观测模型，我们需要知道线特征从归一化平面到像素平面的投影内参矩阵 $\mathcal{K}$ 。如图2，点 $C$ 和 $D$ 是直线 $\mathcal{L}=({\mathbf{n}}^{?},{\mathbf{d}}^{?}{)}^{?}$ 上两点，点 $c$ 和 $d$ 是它们在像素平面上的投影。 $c=KC$,$d=KD$ ,$K$是相机的内参矩阵。 $\mathbf{n}=[C{]}_{\times }D，l=\left[\begin{array}{ccc}{l}_1& l_2& l_3\end{array}\right]=[c{]}_{\times }d$ 。那么有

$$l=\mathcal{K}\mathbf{n}=\left[\begin{array}{ccc}{f}_y& 0& 0\\ 0& f_x& 0\\ ?f_y{c}_x& ?f_x{c}_y& f_x{f}_y\end{array}\right]\mathbf{n}$$

c叉乘d是法向量，但同时也代表了归一化平面上和这个法向量过原点决定的平面的交线。直线的一般式ax+by+c=0。那么归一化平面上的c和d之间所有的点（x,y）点乘它们的法向量n，也是等于0的。xn1+yn2+n3=0。那么n1，n2，n3就是a，b，c。

$$\begin{array}{rl}{l}^i=c{}^i\times d{}^i& =(KC{}^n)\times (KD{}^n)\\ & ={\left[\begin{array}{c}fx{X}_C+cx\\ fy{Y}_C+cy\\ 1\end{array}\right]}_{\times }\left[\begin{array}{c}fx{X}_D+cx\\ fy{Y}_D+cy\\ 1\end{array}\right]\\ & =\left[\begin{array}{ccc}0& ?1& (fy{Y}_C+cy)\\ 1& 0& ?(fx{X}_C+cx)\\ ?(fy{Y}_C+cy)& (fx{X}_C+cx)& 0\end{array}\right]\left[\begin{array}{c}fx{X}_D+cx\\ fy{Y}_D+cy\\ 1\end{array}\right]\\ & =\left[\begin{array}{c}fy(Y_C?Y_D)\\ fx(X_C?X_D)\\ fxfy(X_C{Y}_D?Y_C{X}_D)+fxcy(X_D?X_C)+fycx(Y_D?Y_C)\end{array}\right]\\ & =\left[\begin{array}{ccc}fy& 0& 0\\ 0& fx& 0\\ ?fycx& ?fxcy& fxfy\end{array}\right]\left[\begin{array}{c}{Y}_D?Y_C\\ X_D?X_C\\ X_C{Y}_D?Y_C{X}_D\end{array}\right]\\ & =\left[\begin{array}{ccc}fy& 0& 0\\ 0& fx& 0\\ ?fycx& ?fxcy& fxfy\end{array}\right]{\left[\begin{array}{c}{X}_C\\ Y_C\\ 1\end{array}\right]}_{\times }\left[\begin{array}{c}{X}_D\\ Y_D\\ 1\end{array}\right]\\ & =\mathcal{K}(C{}^n\times D{}^n)\\ & =\mathcal{K}\mathbf{n}\end{array}$$

上式表明，直线的线投影只和法向量有关和方向向量无关。

关于投影的误差，我们不可以直接从两幅图像的线段中得到，因为同一条直线在不同图像线段的长度和大小都是不一样的。衡量线的投影误差必须从空间中重投影回当前的图像中才能定义误差。在给定世界坐标系下的空间直线 ${\mathcal{L}}_l^w$ 和正交表示 ${\mathcal{O}}_l$ ，我们首先使用外参（这也是我们需要优化求解的东西） $T_{cw}=\left[\begin{array}{cc}{R}_{cw}& p_{cw}\\ 0& 1\end{array}\right]$ 将直线变换到相机归一化平面下的观测 $c_i$ 坐标下。然后再将直线利用相机内参投影到成像平面上得到投影线段 $l_l^{c_i}$ ，然后我们就得到了线的投影误差。我们将线的投影误差定义为图像中观测线段的端点到从空间重投影回像素平面的预测直线的距离。

线段从世界坐标系转相机归一化平面坐标系：
$$\begin{array}{c}\left[\begin{array}{l}{\mathbf{n}}_c\\ {\mathbf{d}}_c\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_{cw}& {\left[{\mathbf{t}}_{cw}\right]}_{\times }{\mathbf{R}}_{cw}\\ 0& {\mathbf{R}}_{cw}\end{array}\right]\left[\begin{array}{l}{\mathbf{n}}_w\\ {\mathbf{d}}_w\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_{wc}^T& {\left[?{\mathbf{R}}_{wc}^T{\mathbf{t}}_{wc}\right]}_{\times }{\mathbf{R}}_{wc}^T\\ 0& {\mathbf{R}}_{wc}^T\end{array}\right]\left[\begin{array}{l}{\mathbf{n}}_w\\ {\mathbf{d}}_w\end{array}\right]\\ =\left[\begin{array}{cc}{\mathbf{R}}_{wc}^T& {\mathbf{R}}_{wc}^T{\left[{\mathbf{t}}_{wc}\right]}_{\times }\\ 0& {\mathbf{R}}_{wc}^T\end{array}\right]\left[\begin{array}{l}{\mathbf{n}}_w\\ {\mathbf{d}}_w\end{array}\right]\end{array}$$

• 最后一步推导用到了一个性质，该性质在李群中比较常见，即：当 $M\in \mathbf{SO}(3)$ 时，有 $[Mu{]}_{\times }=M[u{]}_{\times }{M}^T。$

残差定义：

$$\begin{gathered} {\mathbf{r}}_l\left({\mathbf{z}}_{{\mathcal{L}}_l}^{c_i},\mathcal{X}\right)=\left[\begin{array}{l}d\left({\mathbf{s}}_l^{c_i},{\mathbf{l}}_l^{c_i}\right)\\ d\left({\mathbf{e}}_l^{c_i},{\mathbf{l}}_l^{c_i}\right)\end{array}\right] \\ d(\mathbf{s},1)=\frac{{\mathbf{s}}^{?}\mathbf{l}}{\sqrt{l_1^2+l_2^2}} \end{gathered}$$

其中 ${\mathbf{s}}_l^{c_i}$ 和 ${\mathbf{e}}_l^{c_i}$ 是图像中观测到的线段端点， ${\mathbf{l}}_l^{c_i}$ 是重投影的预测的直线。

double FeatureManager::reprojection_error( Vector4d obs, Matrix3d Rwc, Vector3d twc, Vector6d line_w ) {

    double error = 0;

    Vector3d n_w, d_w;
    n_w = line_w.head(3);
    d_w = line_w.tail(3);

    Vector3d p1, p2;
    p1 << obs[0], obs[1], 1;
    p2 << obs[2], obs[3], 1;
	// 根据外参将line从世界坐标系转到相机归一化平面坐标系
    Vector6d line_c = plk_from_pose(line_w,Rwc,twc);
    Vector3d nc = line_c.head(3);
    double sql = nc.head(2).norm();
    nc /= sql;

    error += fabs( nc.dot(p1) );
    error += fabs( nc.dot(p2) );
	
    return error / 2.0;
}

这里误差是归一化平面坐标系的误差，因此观测也应该要求是归一化平面，注意中间有个从像素坐标系到归一化平面坐标系的转换，这里没列出来。

误差求解函数在这里。

这个函数实际上只用在了外点剔除这里，真正的优化误差求解是在优化器那里定义的。而且感觉这里的实现坐标有点问题？

4.2.误差雅克比推导

如果要优化的话，需要知道误差的雅克比矩阵：

线特征在VIO下根据链式求导法则：

$${\mathbf{J}}_l=\frac{?{\mathbf{r}}_l}{?{\mathbf{l}}^{c_i}}\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}\left[\frac{?{\mathcal{L}}^{c_i}}{?\delta {\mathbf{x}}^i}\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}\right]$$

其中第一项 $\frac{?{\mathbf{r}}_l}{?{\mathbf{l}}^{c_i}}$ ，因为

$$\begin{gathered} {\mathbf{r}}_l=\left[\begin{array}{c}\frac{{\mathbf{s}}^T\mathbf{l}}{\sqrt{l_1^2+l_2^2}}\\ \frac{{\mathbf{e}}^T\mathbf{l}}{\sqrt{l_1^2+l_2^2}}\end{array}\right]=\left[\begin{array}{c}\frac{u_s{l}_1+v_s{l}_2}{\sqrt{l_1^2+l_2^2}}\\ \frac{u_e{l}_1+v_e{l}_2}{\sqrt{l_1^2+l_2^2}}\end{array}\right] \\ \mathbf{s}=\left[\begin{array}{ccc}{u}_s& v_s& 1\end{array}\right] \\ \mathbf{e}=\left[\begin{array}{ccc}{u}_e& v_e& 1\end{array}\right] \\ \mathbf{l}=\left[\begin{array}{ccc}{l}_1& l_2& l_3\end{array}\right] \end{gathered}$$

所以：

$$\begin{array}{rl}\frac{?{\mathbf{r}}_l}{?\mathbf{l}}& =\left[\begin{array}{lll}\frac{?r_1}{?l_1}& \frac{?r_1}{?l_2}& \frac{?r_1}{?l_3}\\ \frac{?r_2}{?l_1}& \frac{?r_2}{?l_2}& \frac{?r_2}{?l_3}\end{array}\right]\\ & ={\left[\begin{array}{ccc}\frac{?l_1{\mathbf{s}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{u_s}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{?l_2{\mathbf{s}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{v_s}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}\\ \frac{?l_1{\mathbf{e}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{e_s}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{?l_2{\mathbf{e}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{v_e}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}\end{array}\right]}_{2\times 3}\end{array}\text{\hspace{1em}?}$$

第二项 $\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}$（像素坐标到相机归一化坐标，相差一个映射矩阵） ，因为

$$\begin{gathered} \mathbf{l}=\mathcal{K}\mathbf{n} \\ \mathcal{L}=\left[\begin{array}{cc}\mathbf{n}& \mathbf{d}\end{array}\right] \end{gathered}$$

所以：

$$\begin{array}{rl}\frac{?{\mathrm{l}}^{c_i}}{?{\mathcal{L}}_i^{c_i}}& =\left[\begin{array}{cc}\frac{?\mathbf{l}}{\mathbf{n}}& \frac{?\mathbf{l}}{\mathbf{d}}\end{array}\right]\\ & ={\left[\begin{array}{ll}\mathcal{K}& 0\end{array}\right]}_{3\times 6}\end{array}\text{\hspace{1em}?}$$

最后一项矩阵包含两个部分，一个是相机坐标系下线特征对的旋转和平移的误差导数，第二个是直线对正交表示的四个参数增量的导数

第一部分中，

$$\delta {\mathbf{x}}_i=\left[\delta \mathbf{p},\delta \mathbf{\theta },\delta \mathbf{v},\delta {\mathbf{b}}_a^{b_i},\delta {\mathbf{b}}_g^{b_i}\right]$$

在VIO中，如果要计算线特征的重投影误差，需要将在世界坐标系 $w$ 下的线特征变换到IMU坐标系 $b$ 下，再用外参数 ${\mathbf{T}}_{\mathbf{bc}}$ 变换到相机坐标系 $c$ 下。所以

$$\begin{array}{rl}{\mathcal{L}}_c& ={\mathcal{T}}_{bc}^{?1}{\mathcal{T}}_{wb}^{?1}{\mathcal{L}}_w\\ & ={\mathcal{T}}_{bc}^{?1}{\left[\begin{array}{c}{\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+\left[{\mathbf{d}}^w\right]\times {\mathbf{p}}_{wb}\right)\\ {\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w\end{array}\right]}_{6\times 1}\end{array}$$

其中
$$\begin{gathered} {\mathcal{T}}_{bc}=\left[\begin{array}{cc}{\mathbf{R}}_{bc}& {\left[{\mathbf{p}}_{bc}\right]}_{\times }{\mathbf{R}}_{bc}\\ 0& {\mathbf{R}}_{bc}\end{array}\right] \\ {\mathcal{T}}_{bc}^{?1}=\left[\begin{array}{cc}{\mathbf{R}}_{\mathbf{bc}}^{?}& ?{\mathbf{R}}_{\mathbf{bc}}^{?}[{\mathbf{p}}_{\mathbf{bc}}{]}_{\times }\\ 0& {\mathbf{R}}_{\mathbf{bc}}^{?}\end{array}\right] \end{gathered}$$
$$?[a{]}_{\times }b=[b{]}_{\times }a$$
线特征 $\mathcal{L}$ 只优化状态变量中的位移和旋转，所以只需要对位移和旋转求导，其他都是零。下面我们来具体分析旋转和位移的求导。首先是线特征对旋转的求导：

$$\begin{array}{rl}\frac{?{\mathcal{L}}_c}{?\delta {\theta }_{b{b}^{\prime }}}& ={\mathcal{T}}_{bc}^{?1}\left[\begin{array}{c}\frac{?\left(\mathbf{I}?{\left[\delta {\mathbf{\theta }}_{b{b}^{\prime }}\right]}_{\times }\right){\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+{\left[{\mathbf{d}}^w\right]}_{\times }{\mathbf{p}}_{wb}\right)}{?\delta {\mathbf{\theta }}_{b{b}^{\prime }}}]\\ \frac{?\left(\mathbf{I}?{\left[\delta {\mathbf{\theta }}_{b{b}^{\prime }}\right]}_{\times }^{?}\right){\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w}{?\delta {\mathbf{\theta }}_{b{b}^{\prime }}}\end{array}\right]\\ & ={\mathcal{T}}_{bc}^{?1}{\left[\begin{array}{c}{\left[{\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+{\left[{\mathbf{d}}^w\right]}_{\times }{\mathbf{p}}_{wb}\right)\right]}_{\times }]\\ {\left[{\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w\right]}_{\times }\end{array}\right]}_{6\times 3}\end{array}\text{\hspace{1em}?}$$

然后是线特征对位移的求导：

$$\begin{array}{rl}\frac{?{\mathcal{L}}_c}{?\delta {\mathbf{p}}_{\mathbf{b}{\mathbf{b}}^{\prime }}}& ={\mathcal{T}}_{bc}^{?1}\left[\begin{array}{c}\frac{?{\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+{\left[{\mathbf{d}}^w\right]}_{\times }\left({\mathbf{p}}_{wb}+\delta {\mathbf{p}}_{b{b}^{\prime }}\right)\right)}{?\delta {\mathbf{p}}_{b{b}^{\prime }}}\\ \frac{?{\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w}{?\delta {\mathbf{p}}_{b{b}^{\prime }}}\end{array}\right]\\ & ={\mathcal{T}}_{bc}^{?1}{\left[\begin{array}{c}{\mathbf{R}}_{wb}^{?}{\left[{\mathbf{d}}^w\right]}_{\times }\\ 0\end{array}\right]}_{6\times 3}\end{array}\text{\hspace{1em}?}$$

第二部分中 $\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}$ ，先解释第一个 $\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}$

$${\mathcal{L}}^c={\mathcal{T}}_{wc}^{?1}{\mathcal{L}}^w$$

所以 $\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}={\mathcal{T}}_{wc}^{?1}$

然后后面的 $\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}$ 有两种思路，先介绍第一种：

$$\begin{gathered} \frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}=\left[\begin{array}{cccc}\frac{?{\mathcal{L}}^w}{?{\psi }_1}& \frac{?{\mathcal{L}}^w}{?{\psi }_2}& \frac{?{\mathcal{L}}^w}{?{\psi }_3}& \frac{?{\mathcal{L}}^w}{??}\end{array}\right] \\ \frac{?{\mathcal{L}}^w}{?{\psi }_1}=\frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_1} \\ \frac{?{\mathcal{L}}^w}{??}=\frac{?{\mathcal{L}}^w}{?\mathbf{w}}\frac{?\mathbf{w}}{?{?}_1} \end{gathered}$$

其中 $\mathcal{L}$ 对 $\mathbf{U}$ 和 $\mathbf{w}=[w_1,w_2]$ 求导，因为 ${\mathcal{L}}^w={\left[\begin{array}{cc}{w}_1{\mathbf{u}}_1^{?}& w_2{\mathbf{u}}_2^{?}\end{array}\right]}^{?}$ ，所以

$$\begin{array}{rl}\frac{?\mathcal{L}}{?\mathbf{U}}& ={\left[\begin{array}{ccc}\frac{?\mathcal{L}}{?{\mathbf{U}}_1}& \frac{?\mathcal{L}}{?{\mathbf{U}}_2}& \frac{?\mathcal{L}}{?{\mathbf{U}}_3}\end{array}\right]}_{6\times 9}\\ & =\left[\begin{array}{ccc}{w}_{1(3\times 3)}& 0& 0\\ 0& w_{2(3\times 3)}& 0\end{array}\right]\end{array}\text{\hspace{1em}?}$$

$$\begin{array}{rl}\frac{?\mathcal{L}}{?\mathbf{w}}& ={\left[\begin{array}{cc}\frac{?\mathcal{L}}{?w_1}& \frac{?\mathcal{L}}{?w_2}\end{array}\right]}_{6\times 2}\\ & =\left[\begin{array}{cc}{\mathbf{u}}_1& 0\\ 0& {\mathbf{u}}_2\end{array}\right]\end{array}\text{\hspace{1em}?}$$

然后是 $\mathbf{U}$ 对 $\psi$ 和 $\mathbf{W}$ 对 $?$ 的求导，

因为 $\begin{array}{rl}{\mathbf{U}}^{\prime }& \approx \mathbf{U}\left(\mathbf{I}+[\delta \psi {]}_{\times }\right)\end{array}$ ，所以

$$\begin{gathered} {\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}^{\prime }=\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]+{\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}_{\times }\delta \psi \\ \frac{{\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}^{\prime }?\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}{\delta \psi }={\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}_{\times } \\ \frac{?\mathbf{U}}{?{\psi }_1}=\left[\begin{array}{ccc}0& {\mathbf{u}}_3& ?{\mathbf{u}}_2\end{array}\right] \\ \frac{?\mathbf{U}}{?{\psi }_2}=\left[\begin{array}{ccc}?{\mathbf{u}}_3& 0& {\mathbf{u}}_1\end{array}\right] \\ \frac{?\mathbf{U}}{?{\psi }_1}=\left[\begin{array}{ccc}{\mathbf{u}}_2& ?{\mathbf{u}}_1& 0\end{array}\right] \\ \frac{?\mathbf{w}}{??}=\left[\begin{array}{c}?w_2\\ w_1\end{array}\right] \end{gathered}$$

所以，可得

$$\begin{array}{rl}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}& =\left[\begin{array}{cccc}\frac{?{\mathcal{L}}^w}{?{\psi }_1}& \frac{?{\mathcal{L}}^w}{?{\psi }_2}& \frac{?{\mathcal{L}}^w}{?{\psi }_3}& \frac{?{\mathcal{L}}^w}{??}\end{array}\right]\\ & =\left[\begin{array}{cccc}\frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_1}& \frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_2}& \frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_3}& \frac{?{\mathcal{L}}^w}{?\mathbf{w}}\frac{?\mathbf{w}}{??}\end{array}\right]\\ & ={\left[\begin{array}{cccc}0& ?w_1{\mathbf{u}}_3& w_1{\mathbf{u}}_2& ?w_2{\mathbf{u}}_1\\ w_2{\mathbf{u}}_3& 0& ?w_2{\mathbf{u}}_1& w_1{\mathbf{u}}_2\end{array}\right]}_{6\times 4}\end{array}\text{\hspace{1em}?}$$