【代码阅读】PL-VIO

〇、写在前面

PL-VIO采用的通信是ROS，所以并不能像ORBSLAM那样按照执行顺序来理顺，因为ORBSLAM是有一个真正意义上的主函数的，经过CMakeList的编辑产生的可执行文件会有一个开始，但是PL-VIO用的是ROS，其内部通信是节点与节点之间的通信，所以每个节点都有一个主函数，这里我们将只看PL-VIO中关于线特征相关的部分。

一、linefeature_tracker节点

这个节点负责的是线特征的提取以及帧之间的跟踪，对应的这部分的代码实现是在linefeature_tracker.cpp这个文件里，关于ROS的节点启动相关的内容则放在了lnefeature_tracker_node.cpp里面。

节点主函数部分

在这里，我们将lnefeature_tracker_node.cpp当作主函数来看待，那么程序的入口就在这个文件里面。这个文件的结构其实并不复杂，定义了一堆全局变量，函数的部分只有两个：ROS的回调函数和节点的主函数。

先来看主函数，由于需要使用ROS进行通信，所以必然要写一些ROS相关的语句，包括初始化ROS节点、新建句柄等操作。之后主函数就定义了当前节点需要监听的话题和回调函数，同时定义了节点要向哪一个话题发布消息，从而让处理完的信息流传出去，最后调用spin开始等待ROS的消息传来。从写法上来看，当前这个节点监听的TOPIC是IMAGE_TOPIC，这个话题发布的信息应该是ROS格式的图片，传来图片时表示新的一帧图像到来，和直接用OPENCV读取图像然后调用函数的效果是一样的。

接下来看回调函数的部分，在img_callback函数中，传入的参数是一个ROS格式的图片，并没有函数的返回值。函数首先对发布频率做了限制，只有当图像传来的频率低于一定值的时候，才能够将处理之后的结果再向下一个TOPIC发布，这样子应该是为了保证消息通信的连续性和稳定性，从代码可以看出，如果将PUB_THIS_FRAME设置为了false，线特征的计算会正常进行，但是不会有新的消息发布出去。

之后代码用了一个以前没见过的cv_bridge库，查了一下，这个其实是ROS里面的一个库，因为ROS的图片格式和OPENCV的图片格式是不一样的，所以在用ROS实现一些大程序的时候，涉及一个图像格式转换的问题，所以ROS干脆就直接把库的接口准备好了，调用cv_bridge::toCvCopy，可以直接将回调函数传入的ROS格式的图像转换为一个OPENCV格式的图像，可以用cv::Mat将转换结果读取出来从而送入后续OPENCV的处理中。

接下来对于单目相机来说，就是调用LineFeatureTracker对象来处理线特征，LineFeatureTracker对象是线特征这个节点的一个全局变量，其对应的文件就是linefeature_tracker.cpp，在回调函数中就只用了一个readImage，实际的处理都写在了另一个文件里面。处理完的消息则通过ROS机制，写入一个message里面发布到话题中去。

在向其它TOPIC发送信息的时候，代码里面使用了两个TOPIC，分别是pub_img和pub_match，但是全局搜索之后发现有用的只有pub_img，至于pub_match，代码里面只定义了这个话题的句柄，但是并没有向其发送消息，更没有其他节点接收这个TOPIC的信息。向pub_img发送的消息是sensor_msgs::PointCloudPtr，是ROS自带的消息格式，从名字来看其本质用途可能是传递点云，但这里用来传递匹配的线特征。PointCloudPtr消息包括三部分：头部、点数组和channel数组。其中点数组在这里用来存放每个线段的起点的归一化平面上的坐标，channel数组则灵活一些，里面存放到是三个向量，分别保存了线段的ID、终点的归一化横纵坐标。可以说这里也是在魔改PointCloudPtr的消息类型，利用一些额外的数据结构存放线段信息，以此来避免单独新建其它的消息类型，直接使用ROS自带的消息类型来传递消息。

线特征的提取与跟踪

关键的部分其实位于linefeature_tracker.cpp，在ROS的回调函数中我们将转换后的OPENCV格式的图像送入了这里面，所有线特征的处理就都在这部分中了。我们从readImage开始，进入这个函数之后，首先进行了一次图像的重映射，所谓图像重映射，简单来说就是按照某种规则，将图像中的像素转移到另一个位置，这里实现调用的是OPENCV里面自带的remap函数，这一步应该是在进行图像的畸变校正，根据函数的参数可以看出，校正的规则是直接传入的，而这部分规则的设定是在点特征的节点里面的，这里我们就认为对传入的OPENCV格式的图形进行了全局一致的畸变校正。看到这里其实有点小疑问，既然设计了畸变校正的部分，那么为什么不直接在ROS通信的过程中传递校正过后的图像信息呢，这种每次用都矫正一次必然会让时间开销增大，个人的猜测是ROS消息格式的问题，因为畸变校正调用的是OPENCV提供的函数，所以其处理的必然是OPENCV格式的图像，但这种图像没法用ROS传递，所以PL-VIO采用这种用一次校正一次的方法，干脆将校正放在使用图像之前，可能校正的时间增加了，但相比于在图像的格式之间转换来转换去可能时间开销还要小一些。

之后代码添加了一个根据参数EQUALIZE判断是否进行校正的分支，检查了一下代码的注释部分，当图像过黑或者过亮的时候这个参数就会设置成1，从而进入校正的分支，在校正的过程中，使用了OPENCV自带的cv::createCLAHE校正，它对应的校正方法是自适应直方图均衡（CLAHE) 。
这里补充一下直方图均衡化和自适应直方图均衡化，从时间前后来看，先有直方图均衡化，自适应直方图均衡化是在直方图均衡化的基础上做的改进。直方图均匀化属于一种图像处理技术，通过更新图像直方图的像素强度分布来调整图像的全局对比度，从而让对比度低的区域在输出图像中获得更高的对比度，处理过程中首先计算图像的像素强度的直方图，对直方图中计数最大的区间，均匀展开到其它区间，也就是把原始图像的灰度直方图从比较集中的某个灰度区间变成在全部灰度范围内的均匀分布。

由于图片明暗分布的问题，对一张图片进行全局的直方图均衡化可能导致明部或者暗部的细节丢失。为了优化均衡化效果，我们对可以对不同区域进行直方图均衡化以获得更加合适的效果。和普通的直方图均衡算法不同，自适应直方图均匀化通过计算图像的局部直方图，然后重新分布亮度来来改变图像对比度。因此，该算法更适合于改进图像的局部对比度以及获得更多的图像细节。但是这种划分区块的方法带来的弊端也很明显，就是处理完的图片会出现区块效应。

这两张图分别是直方图均衡化和自适应直方图均衡化后的结果，可以看出直方图均衡化的结果整体都灰蒙蒙的，树的阴影部分明显不如自适应直方图均衡化的结果，反过来自适应直方图均衡化的结果虽然在暗部表现较好，但是因为划分过区域，所以明显像是拼起来的结果。这里我们简单理解，直方图均衡化和自适应直方图均衡化都是图像处理的一种方法，二者区别在于直方图均衡化是对整个图像做直方图然后均衡化，而自适应直方图均衡化是对图像划分格子，每个格子内做直方图然后均衡化。

回到代码本身，对图像做直方图均匀化之后，如果是系统开始的第一帧，就会将当前帧和上一帧都设置为传入的这一帧图像，正常情况只重新初始化当前帧，代码这种写法是因为整个SLAM流程重复使用的两个帧对象，每次都清空然后新赋值，所以是用的reset函数。

接下来进入到正式的图像处理部分，首先调用OPENCV自带的线段提取函数，选择提取方式为LSD，创建LSD提取器，对当前这张图像进行线段的提取，提取时调用的函数是detect，如果要更换线段提取的方法，可以将这里替换掉。

提取完成后，利用BinaryDescriptor::createBinaryDescriptor进行线特征描述子的构建，调用这个对象的compute函数，可以直接计算线段的描述子，函数有三个参数：传入的图像、提取出的线段以及存放描述子的数据结构。

点特征中并不是所有的点都可以作为特征点，一些效果好的才会作为特征点，在线特征中也同样采用了这种策略，代码中使用了一种根据长度和金字塔层数来筛选的方法，如果线段是在第一层提取到的且长度超过30个像素，就会被设置为关键线。这些关键线及其描述子会被存放在当前帧的数据结构中。

对于存放进当前帧的关键线们，会根据当前帧的序号给线赋一个序号，这部分应该是为了保证对应同一条现实中的线的ID相同，在第一帧初始化的时候，假设所有提取到的关键线都可以被三角化恢复空间结构，这样对后序采用同样的对应方式，就可以找到同一条现实中的线在不同帧上的投影。

对于第一帧，程序执行到这里其实就结束了，因为缺少后续的匹配关系，无法三角化恢复线段的空间结构；对于后续的帧，也就是第二帧及其后续的帧来说，则需要进行匹配、三角化等操作。在后续帧的情况下，上一帧用curframe_表示，当前帧则用forwframe_表示，这里必须吐槽一下命名规则，cur居然不是current，而这个forw是什么的缩写都想不出来。

首先需要进行两帧之间线段的匹配，这里的代码使用的是OPENCV中自带的BinaryDescriptorMatcher对象，利用其自带的match函数，传入的参数包括三部分：上一帧线段的描述子、当前帧线段的描述子以及存放匹配结果的数据结构，这个数据结构是一个DMatch组成的向量，DMatch简单来说可以看成一个pair，当然实际上是比一个pair多很多东西的，比如存放描述子距离的distance，但里面最常用的就是queryIdx和trainIdx这两个量，对应两张图上匹配线的ID，所以完全可以将DMatch看作一个pair。

由于BinaryDescriptorMatcher对象使用的匹配机制是暴力匹配，完全依赖于描述子距离，所以其匹配结果会存在一定的误匹配，我们的做法是在结果上做二次筛选，这里用到了存放在DMatch里面的distance属性，首先根据描述子距离来筛选，超过阈值则直接排除。之后再根据线段的两个端点做筛选，匹配线段的两个端点移动的距离不能超过阈值，否则也认为是误匹配。关于这个匹配策略，个人感觉稍微有点简单，尤其是根据移动距离的筛选，这种方式其实是限制了线段在两帧图像之间的移动幅度，但这个移动幅度是人为设置的，也就是说，一旦相机的移动幅度超过了人为设置的阈值，会有很多的正确匹配会被误筛，个人感觉这里更好的修改方法是利用全局一致性筛选，线段的移动幅度应该是保持全局一致的，这样筛选能够保证筛选效果更好，但相应地时间开销也会变大。

对于经过筛选的线段，也就是good_matches里面的DMatch对应的线段，我们需要保证序号的一致性，也就是需要将上一帧的ID赋给当前帧的线段来表示是同一条线。这种方法适用于旧线，也就是上一帧我们赋过ID的线段，对于新线，按照前面的代码执行顺序，其ID应该是-1，这个时候就需要给这些新的匹配线增加一个ID。具体来说，我们将当前帧和上一帧的匹配线进行了ID的统一，如果上一帧的线段使第一次被匹配上，按道理如果上一帧不是初始化帧，这条线段的ID应该是-1，这个时候当前线段的ID也会被改成-1，这时就表明对应的线段是一条新线，我们需要给这对线段分配一个全局的ID，同时将其加入到新线中。

这里区分新线和旧线，实际上是为了进行线的跟踪，当然这个跟踪和ORBSLAM2里面的跟踪不完全一样，这里我们可以将连续的匹配看作是跟踪，对于两帧图像来说，如果上一帧的匹配线段的ID不是-1，意味着存在一个连续的匹配关系一直传递到了当前帧，这时我们就将其看作旧线，反之，如果上一帧的匹配线段的ID是-1，那么说明这对匹配是新建的，就将其看作新线。新线的作用是为旧线做替补，一旦旧线太少，就从新线里面补充，这样做应该是考虑到旧线是连续匹配传递过来的，其特征应该更加稳定。最后旧线会作为关键线存入到当前帧的数据结构中，参与到下一帧的匹配流程中。

函数的最后，还有一个数据类型的转换，当前帧中一条线的表示形式除了前面使用的OPENCV自带的KeyLine类型，还有PL-VIO自己定义的Line类型，函数的最后会将每一条线单独再保存一份Line类型在当前帧的数据结构中。

节点总结

总结一下这个节点，readImage是当前这个节点的主要处理函数，负责线段的提取、匹配，图像经过重映射之后利用OPENCV自带的提取器提取LSD线段并计算描述子，利用BinaryDescriptorMatcher对象的match函数进行暴力匹配，再通过端点移动距离和描述子距离进行误匹配筛选，对于得到的匹配线段，关键在于其ID的维护，PL-VIO的线段跟踪使用的是匹配的传递，对于第一帧，长度和层数符合要求的线段会被当做关键线并赋给其一个全局唯一的ID，对于后续的帧，如果与上一帧产生了匹配，就将当前帧的线段的ID改为与上一帧一致，如果ID是-1，表示这个匹配关系是新建立的，会被标记为新线，反之如果不是-1，则说明线段的匹配关系是从更旧的帧传递过来的，就会被认为是旧线，新线的用处是用来补充旧线，旧线因为特征的传递，所以其特征更具有稳定性。补充后的旧线将作为关键线保存到当前帧的数据结构中参与到后面的计算。

二、vins_estimator节点

linefeature_tracker节点最后将处理后的信息发布在了linefeature这个话题中，所以全局搜索订阅了这个话题的节点，只有vins_estimator这一个节点，所以这里我们顺着进入到这个节点。

节点主函数部分

除了初始化节点的代码，节点的主函数中主要是订阅了四个话题：IMU信息、图像的点特征、图像的线特征以及原始图像。这里个人大胆猜测一波，VINS使用的是点特征，所以其源代码里面用的应该是image来表示点特征，而PL-VIO是在VINS的基础上魔改加入线特征的，所以在这里面用linefeature来表示线特征，而原来VINS使用的代码命名没有修改，所以才会有这种线用line来表示而点用image表示的情况。

我们这里主要查看线特征相关的内容，所以查看linefeature话题的回调函数，这个回调函数简单得离谱，就是在保证互斥锁的前提下，将传入的线特征信息给导入到了一个存放线特征的缓冲区中。最后一句的con是c++中的一个用于实现多个线程间的同步操作的条件变量，调用notify_one会唤醒一个挂起的进程，相关的操作可以参考链接

现在图像提取到的线特征匹配信息被保存到了缓冲区linefeature_buf里面，所以需要顺着去找对这个缓冲区做提取的函数，全局搜索之后可以看出，对这个队列的读取全都在vins_estimator这个节点中，所以我们查看这个节点是没有找错的。顺着找，可以找到getMeasurements这个函数。

getMeasurements函数

从名字来推测，这个函数是用来获得一些测量值的，函数的返回值的写法也很暴力，直接就是一个向量，向量内部的元素是套了好几层的pair，简单来说就是把一个存放有IMU信息、特征点信息和特征线信息的向量返回了。从函数的代码结构来看，里面就只有一个死循环，当存放有IMU信息、点特征、线特征的数据结构非空的时候就读取出来，否则直接返回。

这里我们只看线特征的部分，也就是linefeature_buf，取出来的线特征放在了linefeature_msg里面，每次循环取出一条，取出来的信息被存放在measurements里面，最后返回给调用getMeasurements函数的位置。其实其它的特征也是采用了同样的方法，都是将队列里面的消息取出，然后更换数据结构后在传回。

关于这个函数，额外插一嘴就是时间同步的问题，因为在这个函数中三种信息是同时提取的，点特征和线特征一般是同步的，因为这两个信息来源是同一张图像，其到达缓冲区的时间也应该接近，而IMU信息则有可能与点线特征不同步，这个时候就需要判断去留的问题，这里主要是利用ROS的信息里面的头部，根据头部中时间戳来判断对哪部分信息做保留。如果IMU最新数据的时间戳不大于最旧图像的时间戳，意味着当前我有的最新的IMU信息，都要被点线特征的信息旧，说明IMU的信息不能用了，需要等待IMU传来更新的消息。反之，如果IMU最老的数据时间戳不小于最旧图像的时间，说明我当前拥有最旧的IMU信息要比最旧的点线信息要新，表明点线信息太旧了，应该换稍微新一点的点线信息。

所以对于getMeasurements函数来说，它负责的其实是一个信息的中转，将IMU信息、点特征、线特征进行整合，在保证时间间隔不太大的情况下，将其转发给调用它的函数。所以接下来需要对就是找谁调用了getMeasurements函数。顺着找，我们发现是process函数调用了getMeasurements，而process是在vins_estimator节点中单独开辟出来的一个线程进行执行的，所以接下来我们转到process函数里。

process函数

前面调用的getMeasurements函数，返回值是一个向量，当返回的向量的大小不为零时，说明获得了一批符合条件的信息，可以开始处理了，所以这是process函数就结束挂起，开始进行处理。

之后读取传过来的信息，由于传过来的信息实际上是一个嵌套的pair，所以读取时，第一层的first对应的是IMU信息，第一层的second也是一个pair，是整合了点线信息的pair，再拆开这个pair才能得到点线信息，这个第二层pair中first是点信息，second是线段信息。
读取的信息被分别送到对应的函数进行后续的处理，IMU信息送入send_imu函数经过格式转换送到processIMU进行后续处理。对于点线特征，它们的处理都是在processImage里面，在传入之前，用了两个map去进行格式的转换。

image和lines本身是一个map，拿线特征来说，其索引是feature_id，对于单目相机来说，NUM_OF_CAM的值为1，所以feature_id就是之前我们提到过的全局唯一的线的ID，所以这里，lines[feature_id]表示的就是同一条线在不同帧上的投影结果，具体来说就是投影的端点信息。将点线的投影信息的格式处理好之后，就送入processImage函数进行处理。
从后续的代码来看，并没有其它的处理，而是直接向对应的TOPIC发送处理结果，所以线特征的处理应该就在processImage里面，我们再转而进入到这个函数中。关于process这个函数，我们可以理解为是一个信息的分发中转站，getMeasurements将不同来源的信息凑在了一起送到process里面，再有process拆分开送到不同的处理模块。个人的疑惑在于，为什么要采用这种打包再拆包的方法，数据的读取并不需要特别复杂的代码，如果是为了可读性，没必要牺牲打包拆包的时间，感觉process和getMeasurements完全可以整合为一个函数。

processImage函数

这个函数在estimator.cpp中重载了三次，个人猜测是PL-VIO的作者在写代码时直接在VINS的基础上做了重载而不是重写函数，根据函数的参数，PL-VIO使用的函数应该是三个形参的，分别为点特征、线特征以及信息的头部。

进入函数后，首先会存储检测出来的特征，通过调用addFeatureCheckParallax函数，存储的过程本身就是一个根据ID判断是否存在旧特征的过程，点和线的操作实际上是一样的，取出传入的特征，搜索当前存储的特征，如果之前存过这个特征，就将新的特征存储进对应的向量，反之如果没有存过，那就新建一个特征。

这段代码是线特征的存储，里面两个比较难懂的地方，一个是新建特征的位置，这里创建对象使用的构造函数的参数，用了一个很长的写法，这里看的时候个人没怎么看明白，所以就顺着去理了一下结构，这里id_line是遍历的lines里面的每个对象，而lines是传入时使用的线特征的map，其每一个对象应该是map的键值对，它的first对应的是键的部分，也就是特征的ID，值的部分也就是second则是vector<pair<int, Vector4d>>，在这里去向量的第0个元素，得到的就是特征第一次出现时信息，是一个pair，再从pair中去second，得到的就是特征第一次出现时线段在成像平面上的起点和终点坐标。这样正好将传入的参数与构造函数里面的声明对应上了，而且也能对应上feature_id的赋值过程。
另一个难懂的地方是代码142行使用的find_if函数，find_if (begin, end, func)就是从begin开始，到end为止，返回第一个让 func这个函数返回true的iterator，简单来说这里就是从存储的全部线特征中，根据ID找是否存在相同的，如果满足，it的值就为指向对应特征的指针，否则指向对应数据结构的尾部。后续再利用it的结果对特征做新建或者追加操作。

存储好之后函数剩下的部分是在根据特征点的数目等信息，确定边缘化的策略，这部分完全是依赖的点特征，根据匹配点特征的数目以及视差变化情况，确定边缘化策略。但这貌似和VINS论文里提到的边缘化策略不一样，论文里是根据次新帧的情况来判断，而在这代码里面应该算是当前帧，如果代码没错的话，应该是这里判断边缘化策略，当下一帧到来的时候再根据标记的策略来选择对应的边缘化实现。

回到processImage函数，将边缘化策略的标记存储之后，将根据solver_flag的状态来判断是进行初始化还是进行正常的滑动窗口维护，当进入初始化分支时，会先在滑动窗口中积累一定数量的帧，当填满滑动窗口后才会进行后续的操作。无论在初始化还是维护，它们都使用到了solveOdometry函数来三角化新特征，我们的重点在于线特征相关的内容，所以我们直接看solveOdometry函数。

triangulateLine函数

solveOdometry本身并没有太多的操作，它起到一个分支的作用，当solver_flag修改为NON_LINEAR后，也就是初始化完成后，就会进入到三角化的分支中，对于点没有什么好说的，直接使用的就是VINS里面的点三角化内容，而对于线特征，这里做了一个分支，由于我们默认用的是单目相机，所以使用的三角化线段的函数应该是三个参数的triangulateLine。这里triangulateLine函数其实也做了重载，当传感器为双目相机时，使用的triangulateLine函数为一个参数，反之使用的triangulateLine函数为三个参数。

现在终于到了重头戏线段的三角化了，进入到triangulateLine函数中。遍历每个线特征，也就是存放在linefeature里面的从开始运行到当前时刻的所有线特征。在这个循环中首先有两个continue的判断，也就是说能够进行三角化的线段需要有一定的要求，第一个判断可以总结为是观测关系上的判断，如果能够观测到当前线段的帧数少于2或者这个线段是最近才被观测到的，那么就不进行三角化；第二个判断则是防止重复进行三角化，如果is_triangulation为true则表示已经三角化过了，那么不需要重新三角化只需要进行校正优化即可。

在看下面的代码之前，最好先理顺一下函数传入的三个参数是什么，这部分参数涉及到后续的位姿计算。对于第一个参数Ps，其表示的应该是滑动窗口内每一帧的位姿里面的t的部分，而位姿中R的部分则是在初始化的时候通过传地址赋值的。具体来说，在estimator.cpp里面，有一个存储旋转R的矩阵Rs，是Estimator类里面的私有变量，而且是一个完整的矩阵，但是在feature_manager.cpp里面也有一个Rs，这个则是一个指针，指向一个矩阵，而这个连接关系的建立是在feature_manager对象初始化的时候，也就是说在初始化这个对象的时候，将Estimator的Rs的地址传给了feature_manager对象，所以在feature_manager就没有额外的开辟矩阵，但是对于t来说，并没有这样一个操作，就只能通过参数传递的方法来实现相互的读取，这里显然有些写法上的问题，R和t矩阵对于SLAM过程来说基本是同等地位的，所以为什么不直接将这两个参数采用同样的写法，直接用R矩阵那样指针的方法去实现。所以在feature_manager.cpp中，用Ps和Rs来表示滑动窗口内每一帧的t和R，对于它们的迭代更新，放在了estimator.cpp中的slideWindow函数中。

而剩下两个参数tic和ric，表示的则是相机和IMU之间的转换关系，在Estimator.cpp的setParameter函数中进行了初始化，由于是单目相机，所以这里只会初始化一个值。

所以再来看三角化线段的函数，这个函数首先遍历了读取的全部存储的线特征，在可以三角化的情况下，利用t0和R0读取了当前这个特征第一次出现时IMU获得的位姿，后面为了方便我们统一称之为起始帧，之后再将IMU位姿转换为相机的位姿，这里的下标w表示世界坐标系，i表示IMU坐标系，c表示相机坐标系。

之后再遍历这个线特征在非起始帧下的观测，这里的遍历采用了两个指针（不是指针但是用指针好理解），一个固定指向线特征的起始帧，另一个用于指向非起始帧的每一个观测，当二者相等时，表示遍历到了起始帧，不进行三角化操作只存储信息。这里首先是取出当前帧中观测的线段的起点和终点的坐标，之后调用pi_from_ppp函数，函数用来根据三点确立一个平面，这里的三点分别是相机光心、线段的两个端点，函数的返回值是平面方程ax+by+cz+d=0中的四个参数。

关于pipi_plk这个函数，其对应的是论文中普朗克坐标的计算方法，论文中用的表述方法是利用两个平面方程的向量形式叉乘从而得到一个矩阵，从矩阵中可以拆分出普朗克坐标需要的n和d。

对应到代码实现里面，具体的代码实现是放在line_geometry.cpp里面的，这个函数传入的参数是两个四维向量，也就是空间平面方程里面的abcd四个参数，普朗克坐标本身是一个六维向量，其中两个三维向量nd组成了普朗克坐标，在实现的时候，用上面论文里的公式进行计算，得到的是左边的这个矩阵，但普朗克坐标是nd，所以需要从里面拆出来，因此代码实现里面有从dp里面拆元素的这一步操作。

在其余情况下，则是进行线段的三角化，也就是遍历同一条线段在非起始帧中的观测，采用同样的方法，取出观测帧的R和t，从而得到起始帧和当前帧两帧之间的R和t，再利用相同的方法，取出当前帧的线段端点在起始帧中的坐标，恢复一个平面。这里个人感觉并不是在实际计算具体的线段空间表示，而是在筛选最适宜进行三角化的线段观测，代码应该是在重复计算p3和p4，这两个点实际上是观测到的线段在起始帧上的端点的投影，这个时候得到的pij，实际上是转换了帧的延伸平面，根据高中的空间几何，最下面的nj就是平面pij的空间平面法向量，ni则是第一帧的空间平面法向量。

之后代码进行了一个筛选，就是利用这两个法向量，二者点乘取全部的最小值记为最优值，由于两个法向量都是单位向量，所以这里的点乘结果就是法向量夹角的余弦值cos。

根据这个标准得到的最优组合，将会利用pipi_plk得到最后的普朗克坐标。这里必须提一嘴，这个写法完全是在浪费时间，前面循环的时候已经计算过以此了，这里对pij的计算完全是多余的，完全可以直接在取最优值的时候把pij也保存下来，完全没必要在这里浪费时间再计算一次。

现在还有一个需要明确的问题，就是根据cos筛选最优组合的合理性。这里个人有些疑惑，所谓筛选，就是选择最好的两个观测来恢复线段的空间表示，那么这里的最好，应该是在记录的位姿变换下，投影到同一帧之后重合度最大，那么按道理，同一条线段在不同帧上的观测，投影到同一帧，如果位姿计算正确无误，那么这两条线应该是重合的，那么延伸出来的平面也应该是重合的，那法向量指向相同，按这样计算，夹角应该越小越好，也就是cos越大越好，而代码的筛选刚好是反过来的，这一点确实搞不明白为什么。这个问题暂时保留，等回学校和师兄讨论一下。

所以PLVIO中线段的三角化我们就解决了，简单来说就是筛选加计算，筛选是找两个效果比较好的观测来进行三角化，PLVIO固定了一个帧，也就是线段的初始帧，而另一帧则是筛选出来的，用的方法是遍历所有的观测，将观测和初始帧投影到一起，检查延伸平面的法向量的夹角，取夹角的最大值对应的观测帧，之后就用这两个帧进行三角化，利用延伸平面的法向量，根据这两个法向量就可以用论文中的计算方法算出线段的普朗克坐标。

此外，根据论文所说一般情况下空间线段的表示使用的是普朗克坐标，这个坐标的计算是在三角化的过程中实现的，也就是说一条线段只要完成三角化，那么表示线段的数据结构中一定带一个普朗克坐标，而普朗克坐标到正交坐标的转换，则是在优化时进行的，而在具体的表示线段的数据结构中，并没有存储正交坐标，而是在每次需要的时候，都取出普朗克坐标，利用plk_to_orth函数转换为正交坐标，利用正交坐标优化完之后，再转回普朗克坐标并覆盖掉之前的旧数据。