16 Sep 2024
double FeatureManager :: compensatedParallax2 ( const FeaturePerId & it_per_id , int frame_count )
{
//check the second last frame is keyframe or not
//parallax betwwen seconde last frame and third last frame
const FeaturePerFrame & frame_i = it_per_id . feature_per_frame [ frame_count - 2 - it_per_id . start_frame ];
const FeaturePerFrame & frame_j = it_per_id . feature_per_frame [ frame_count - 1 - it_per_id . start_frame ];
double ans = 0 ;
Vector3d p_j = frame_j . point ;
double u_j = p_j ( 0 );
double v_j = p_j ( 1 );
Vector3d p_i = frame_i . point ;
Vector3d p_i_comp ;
//int r_i = frame_count - 2;
//int r_j = frame_count - 1;
//p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
p_i_comp = p_i ;
double dep_i = p_i ( 2 );
double u_i = p_i ( 0 ) / dep_i ;
double v_i = p_i ( 1 ) / dep_i ;
double du = u_i - u_j , dv = v_i - v_j ;
double dep_i_comp = p_i_comp ( 2 );
double u_i_comp = p_i_comp ( 0 ) / dep_i_comp ;
double v_i_comp = p_i_comp ( 1 ) / dep_i_comp ;
double du_comp = u_i_comp - u_j , dv_comp = v_i_comp - v_j ;
ans = max ( ans , sqrt ( min ( du * du + dv * dv , du_comp * du_comp + dv_comp * dv_comp )));
return ans ;
}
feature manager 的主要数据结构为：
class FeatureManager
class FeaturePerId
class FeaturePerFrame
FeaturePerFrame 包括某帧内某个路标被观测的信息，如归一化平面坐标及速度、像素坐标、是否是双目、时间偏移。
FeaturePerId 则主要包括 vector<FeaturePerFrame> feature_per_frame，以及 frame_id、start_frame 等路标点个体级别的信息。值得注意的是，vins假设特征点是被连续观测到的，因此只记录开始的帧，即可推断出 vector 内其他观测所属的帧。
传入 start_frame的变量则是外部的 frameCnt，frameCnt 指明了滑窗内的哪一帧。
主体函数解释
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1»» &image, double td)
输入包括：
frame_count ：指明当前帧将会是滑窗内的哪一帧
image：map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> 的数据结构，记录了当前获得的新特征点集的信息，分级索引，分别是 id、相机id，信息则是点的二维几何信息。
td：与 imu 的时间戳延迟
可分为两大步骤：
添加 image 到 list<FeaturePerId>类型的数据结构中，并作一些统计
判断是否为关键帧
第一步：遍历输入 image 里的点，将其构造成 FeaturePerFrame 类型数据结构，判断该点是否被双目观测，若是则将右目信息也初始化进 FeaturePerFrame。判断 list<FeaturePerId>中是否有当前 image 的 id，不存在则 push 进新 id 并初始化对应的内存，存在则在对应 id 下新增二维几何信息等信息。
for ( auto & id_pts : image )
{
FeaturePerFrame f_per_fra ( id_pts . second [ 0 ]. second , td );
assert ( id_pts . second [ 0 ]. first == 0 );
if ( id_pts . second . size () == 2 )
{
f_per_fra . rightObservation ( id_pts . second [ 1 ]. second );
assert ( id_pts . second [ 1 ]. first == 1 );
}
int feature_id = id_pts . first ;
auto it = find_if ( feature . begin (), feature . end (), [ feature_id ]( const FeaturePerId & it )
{
return it . feature_id == feature_id ;
});
if ( it == feature . end ())
{
feature . push_back ( FeaturePerId ( feature_id , frame_count ));
feature . back (). feature_per_frame . push_back ( f_per_fra );
new_feature_num ++ ;
}
else if ( it -> feature_id == feature_id )
{
it -> feature_per_frame . push_back ( f_per_fra );
last_track_num ++ ;
if ( it -> feature_per_frame . size () >= 4 )
long_track_num ++ ;
}
}
第二步：若是窗口内前两帧，则直接视为关键帧；若追踪点数少于20个，视为关键帧；若连续4帧以上追踪的点少于 40 个，也视为关键帧；若新特征点数目大于追踪特征点的一半，亦认为是关键帧。
之后检查第二新和第三新的帧的特征点视差（前提是这个特征点有第二新和第三新的被观测帧），若平均视差大于阈值，则设置为关键帧。
20 和 40 是不是该调换一下，目前认为不太合理，因为 long_track_num <= last_track_num 是一定成立的（参考上个代码块的最后几行）。
如果面向一面白墙，一帧的特征点追踪个数始终小于阈值，则会被一直判定为关键帧，但这并不是我们的本意。我们的本意是，由于视角移动较大，因此会有一些被追踪的特征点消失在视野内，因此特征点追踪的数目下降，但新增的特征点会多一些。
if ( frame_count < 2 ||
last_track_num < 20 ||
long_track_num < 40 ||
new_feature_num > 0.5 * last_track_num )
return true ;
for ( auto & it_per_id : feature )
{
if ( it_per_id . start_frame <= frame_count - 2 &&
it_per_id . start_frame + int ( it_per_id . feature_per_frame . size ()) - 1 >= frame_count - 1 )
{
parallax_sum += compensatedParallax2 ( it_per_id , frame_count );
parallax_num ++ ;
}
}
if ( parallax_num == 0 )
{
return true ;
}
else
{
ROS_DEBUG ( "parallax_sum: %lf, parallax_num: %d" , parallax_sum , parallax_num );
ROS_DEBUG ( "current parallax: %lf" , parallax_sum / parallax_num * FOCAL_LENGTH );
last_average_parallax = parallax_sum / parallax_num * FOCAL_LENGTH ;
return parallax_sum / parallax_num >= MIN_PARALLAX ;
}
void FeatureManager::triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
遍历特征点，若已有深度则不需要再进行三角，跳过该点。若无深度，则需要进行三角化，且无论双目还是单目，都使用了点被观测的第一帧坐标。若是单目，且特征点只有一个观测，无法三角化，因此也什么都不执行。
若是双目观测，则直接使用双目特征点三角化，不考虑追踪的时间长短 。
若是单目观测，且至少被观测两次，则用 start_frame 和 start_frame+1 这两帧的点作三角化（此时这两帧的 $P$ 也都已知了
），也不考虑追踪的时间长短（追踪连续四帧及以上谓长） 。
代码的核心语句是 triangulatePoint(leftPose, rightPose, point0, point1, point3d);。而前后的代码都是由 imu_T_w 构造出 cam_T_w 作为该函数的输入。
void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1, Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
齐次坐标表示，等号本质上表示等价，成比例，并不是数值上的相等，因此一个点提供两个约束
$p_i$ 是 $P$ 的行，$P_{3\times 4}$ 是世界系到相机系的投影矩阵，到此函数已经把 2D 特征点表示在 归一化平面 了，因此 $P = K[R|t] = [R|t]$。列出投影的齐次方程：
\(x = PX, x'=P'X\)
\(\begin{bmatrix}
x\\
y\\
1
\end{bmatrix}
=
\begin{bmatrix}
p_1\\
p_2\\
p_3\\
\end{bmatrix}
X
,
\begin{bmatrix}
x'\\
y'\\
1
\end{bmatrix}
=
\begin{bmatrix}
p'_1\\
p'_2\\
p'_3\\
\end{bmatrix}
X\)
得到真正的约束
\(x = \frac{p_1X}{p_3X},
y = \frac{p_2X}{p_3X}\)
\(x' = \frac{p'_1X}{p'_3X},
y' = \frac{p'_2X}{p'_3X}\)
分母乘过去，合并同类项
\[(xp_3-p_1)X = 0\\
(yp_3-p_2)X = 0\\
(x'p'_3-p'_1)X = 0\\
(y'p'_3-p'_2)X = 0\\\]
写成矩阵形式
\[\begin{bmatrix}
(xp_3-p_1)\\
(yp_3-p_2)\\
(x'p'_3-p'_1)\\
(y'p'_3-p'_2)\\
\end{bmatrix}
X = 0\]
之后使用 SVD 方法求解最小二乘解，即最小奇异值对应的右奇异向量。
对应的代码如下：
void FeatureManager :: triangulatePoint ( Eigen :: Matrix < double , 3 , 4 > & Pose0 , Eigen :: Matrix < double , 3 , 4 > & Pose1 ,
Eigen :: Vector2d & point0 , Eigen :: Vector2d & point1 , Eigen :: Vector3d & point_3d )
{
Eigen :: Matrix4d design_matrix = Eigen :: Matrix4d :: Zero ();
design_matrix . row ( 0 ) = point0 [ 0 ] * Pose0 . row ( 2 ) - Pose0 . row ( 0 );
design_matrix . row ( 1 ) = point0 [ 1 ] * Pose0 . row ( 2 ) - Pose0 . row ( 1 );
design_matrix . row ( 2 ) = point1 [ 0 ] * Pose1 . row ( 2 ) - Pose1 . row ( 0 );
design_matrix . row ( 3 ) = point1 [ 1 ] * Pose1 . row ( 2 ) - Pose1 . row ( 1 );
Eigen :: Vector4d triangulated_point ;
triangulated_point =
design_matrix . jacobiSvd ( Eigen :: ComputeFullV ). matrixV (). rightCols < 1 > ();
point_3d ( 0 ) = triangulated_point ( 0 ) / triangulated_point ( 3 );
point_3d ( 1 ) = triangulated_point ( 1 ) / triangulated_point ( 3 );
point_3d ( 2 ) = triangulated_point ( 2 ) / triangulated_point ( 3 );
}
void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
首先确保当前帧不是滑窗内的第一帧，因为单目的话，第一帧无法三角化点，也就无法 pnp 了。之后遍历所有特征点 id，选取有深度的点（已被三角化），并判断这个点的被观测帧是否包含当前帧，如果包含，则将其 3D 坐标由世界系转化到相机系，并分别将点在当前帧的 2D 坐标和 3D 坐标放入 vector 容器内。
之后将这些 2D-3D 点对传入函数 if(solvePoseByPnP(RCam, PCam, pts2D, pts3D))，点对数不够则 pnp 失败，返回 false，成功则放回 true，Rcam 和 PCam 会被赋值，表示世界系到相机系的变换，需要再作处理，得到机体系到世界系的变换矩阵。并且 pnp 求解过程会用到 R、t 的初始值，由 imu 提供。
bool FeatureManager::solvePoseByPnP(Eigen::Matrix3d &R, Eigen::Vector3d &P, vector<cv::Point2f> &pts2D, vector<cv::Point3f> &pts3D)
核心语句是 pnp_succ = cv::solvePnP(pts3D, pts2D, K, D, rvec, t, 1);，其前后都是做一些合法判断、矩阵的变换。需要做一些矩阵变换的原因是，最外部输入和最终的输出，均是 w_T_cam（机体坐标相对于世界坐标，cam2w），而 opencv 的 pnp 函数需要的位姿代表的是 cam_T_w。
bool FeatureManager :: solvePoseByPnP ( Eigen :: Matrix3d & R , Eigen :: Vector3d & P ,
vector < cv :: Point2f > & pts2D , vector < cv :: Point3f > & pts3D )
{
Eigen :: Matrix3d R_initial ;
Eigen :: Vector3d P_initial ;
// w_T_cam ---> cam_T_w
R_initial = R . inverse ();
P_initial = - ( R_initial * P );
//printf("pnp size %d \n",(int)pts2D.size() );
if ( int ( pts2D . size ()) < 4 )
{
printf ( "feature tracking not enough, please slowly move you device! \n " );
return false ;
}
cv :: Mat r , rvec , t , D , tmp_r ;
cv :: eigen2cv ( R_initial , tmp_r );
cv :: Rodrigues ( tmp_r , rvec );
cv :: eigen2cv ( P_initial , t );
cv :: Mat K = ( cv :: Mat_ < double > ( 3 , 3 ) << 1 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 1 );
bool pnp_succ ;
pnp_succ = cv :: solvePnP ( pts3D , pts2D , K , D , rvec , t , 1 );
//pnp_succ = solvePnPRansac(pts3D, pts2D, K, D, rvec, t, true, 100, 8.0 / focalLength, 0.99, inliers);
if ( ! pnp_succ )
{
printf ( "pnp failed ! \n " );
return false ;
}
cv :: Rodrigues ( rvec , r );
//cout << "r " << endl << r << endl;
Eigen :: MatrixXd R_pnp ;
cv :: cv2eigen ( r , R_pnp );
Eigen :: MatrixXd T_pnp ;
cv :: cv2eigen ( t , T_pnp );
// cam_T_w ---> w_T_cam
R = R_pnp . transpose ();
P = R * ( - T_pnp );
return true ;
}
void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
滑窗进行边缘化后，特别是边缘化最旧的一帧时， 在内存上的体现是，依次将后序的内容移动到相邻的前一块，这意味着帧的序号发生了变化，此时也要对 feature_manager 里的 start_frame 进行更新。
因此以 id 索引遍历特征点，如果 start_frame 不是第一帧，则减 1；如果 start_frame 是第一帧，则丢弃该 2D 观测，并检查剩余的观测数量是否小于2，若小于则把该点抹去，否则为新的 start_frame 计算深度，方法为重投影：
\[X=P^{-1}x \\
x'=P'X\]
void FeatureManager :: removeBackShiftDepth ( Eigen :: Matrix3d marg_R , Eigen :: Vector3d marg_P , Eigen :: Matrix3d new_R , Eigen :: Vector3d new_P )
{
for ( auto it = feature . begin (), it_next = feature . begin ();
it != feature . end (); it = it_next )
{
it_next ++ ;
if ( it -> start_frame != 0 )
it -> start_frame -- ;
else
{
Eigen :: Vector3d uv_i = it -> feature_per_frame [ 0 ]. point ;
it -> feature_per_frame . erase ( it -> feature_per_frame . begin ());
if ( it -> feature_per_frame . size () < 2 )
{
feature . erase ( it );
continue ;
}
else
{
Eigen :: Vector3d pts_i = uv_i * it -> estimated_depth ;
Eigen :: Vector3d w_pts_i = marg_R * pts_i + marg_P ;
Eigen :: Vector3d pts_j = new_R . transpose () * ( w_pts_i - new_P );
double dep_j = pts_j ( 2 );
if ( dep_j > 0 )
it -> estimated_depth = dep_j ;
else
it -> estimated_depth = INIT_DEPTH ;
}
}
}
}
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
在 gpu 版本里冗余了，并没有实际计算补偿的归一化坐标，因此代码后半段是重复冗余计算的，可改进 。
补充说明一下补偿的思路：有 i, j 两帧，由于仅有旋转运动不利于对极几何求相对位姿，因此补偿的具体操作是把 j 的相机姿态对齐到 i，然后计算出此时的特征点坐标如何，再与上一帧的特征点坐标做差。
有了补偿，考虑两个极端情况：
只有旋转，此时补偿后，特征点在两帧里基本没有移动
只有平移，此时补偿后与补偿前的结果一致
double FeatureManager :: compensatedParallax2 ( const FeaturePerId & it_per_id , int frame_count )
{
//check the second last frame is keyframe or not
//parallax betwwen seconde last frame and third last frame
const FeaturePerFrame & frame_i = it_per_id . feature_per_frame [ frame_count - 2 - it_per_id . start_frame ];
const FeaturePerFrame & frame_j = it_per_id . feature_per_frame [ frame_count - 1 - it_per_id . start_frame ];
double ans = 0 ;
Vector3d p_j = frame_j . point ;
double u_j = p_j ( 0 );
double v_j = p_j ( 1 );
Vector3d p_i = frame_i . point ;
Vector3d p_i_comp ;
//int r_i = frame_count - 2;
//int r_j = frame_count - 1;
//p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
p_i_comp = p_i ;
double dep_i = p_i ( 2 );
double u_i = p_i ( 0 ) / dep_i ;
double v_i = p_i ( 1 ) / dep_i ;
double du = u_i - u_j , dv = v_i - v_j ;
double dep_i_comp = p_i_comp ( 2 );
double u_i_comp = p_i_comp ( 0 ) / dep_i_comp ;
double v_i_comp = p_i_comp ( 1 ) / dep_i_comp ;
double du_comp = u_i_comp - u_j , dv_comp = v_i_comp - v_j ;
ans = max ( ans , sqrt ( min ( du * du + dv * dv , du_comp * du_comp + dv_comp * dv_comp )));
return ans ;
}
16 Sep 2024
void Estimator::processMeasurements()
这个函数单独在一个线程内运行，无限循环，间隔2ms。
提取出各帧的特征点并收到了一系列的 IMU 数据后，就该开始处理这些数据了。
首先判断 featureBuf 队列是否为空，如果是空的，就什么也不做。若不空，则从 featureBuf 里提取出最早的特征点数据帧：
void Estimator :: processMeasurements ()
{
while ( 1 )
{
//printf("process measurments\n");
TicToc t_process ;
pair < double , map < int , vector < pair < int , Eigen :: Matrix < double , 7 , 1 > > > > > feature ;
vector < pair < double , Eigen :: Vector3d >> accVector , gyrVector ;
if ( ! featureBuf . empty ())
{
feature = featureBuf . front ();
curTime = feature . first + td ;
之后如果使用 IMU 且 IMU 数据缓存不够新，则无限循环 “wait for imu …”，否则（不使用 IMU 或 IMU 数据足够新）跳出循环，进行下一步。
while ( 1 )
{
if (( ! USE_IMU || IMUAvailable ( feature . first + td )))
break ;
else
{
printf ( "wait for imu ... \n " );
if ( ! MULTIPLE_THREAD )
return ;
std :: chrono :: milliseconds dura ( 5 );
std :: this_thread :: sleep_for ( dura );
}
}
如果使用 IMU，则从 accBuf 和 gyrBuf 里提取前一帧和当前帧之前的 IMU 数据到 accVector 和 gyrVector 里：
mBuf . lock ();
if ( USE_IMU )
getIMUInterval ( prevTime , curTime , accVector , gyrVector );
featureBuf . pop ();
mBuf . unlock ();
如果使用 IMU，且还未初始化第一帧位姿，则利用加速度计的读数来初始化位姿，并强制 yaw 为 0。如果已经初始化第一帧位姿，则进入 IMU 真正的后端 processIMU() 里面包含预积分和将因子送入因子图。积分离散化使用 零阶反向 。
if ( USE_IMU )
{
if ( ! initFirstPoseFlag )
initFirstIMUPose ( accVector );
for ( size_t i = 0 ; i < accVector . size (); i ++ )
{
double dt ;
if ( i == 0 )
dt = accVector [ i ]. first - prevTime ;
else if ( i == accVector . size () - 1 )
dt = curTime - accVector [ i - 1 ]. first ;
else
dt = accVector [ i ]. first - accVector [ i - 1 ]. first ;
processIMU ( accVector [ i ]. first , dt , accVector [ i ]. second , gyrVector [ i ]. second );
}
}
void Estimator::processIMU()
接下来进入 processIMU() 的内部。首先是判断是否收到过第一帧 IMU 数据，如果没收到，则
void Estimator :: processIMU ( double t , double dt , const Vector3d & linear_acceleration , const Vector3d & angular_velocity )
{
if ( ! first_imu )
{
first_imu = true ;
acc_0 = linear_acceleration ;
gyr_0 = angular_velocity ;
}
介绍一下 IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]：
IntegrationBase *：表示一个指向 IntegrationBase 类对象的指针。
pre_integrations：是一个数组的名称。
[(WINDOW_SIZE + 1)]：定义了数组的大小，即数组有 WINDOW_SIZE + 1 个元素。
如果 !pre_integrations[frame_count] 为空指针，则将 IMU 数据初始化到其中：
if ( ! pre_integrations [ frame_count ])
{
pre_integrations [ frame_count ] = new IntegrationBase { acc_0 , gyr_0 , Bas [ frame_count ], Bgs [ frame_count ]};
}
如果不是滑窗内的第一帧，则可以处理当前帧和上一帧之间的 IMU 数据，具体则是将当前的 imu 数据和上一帧 IMU 数据的时间差、加速度、角速度传入 pre_integrations 和 buf 中（将 IMU 数据缓存到这些ector中的作用是什么呢？）。之后利用 IMU 数据 持续预测当前的姿态，最终将预测出当前特征帧的姿态 。最后保存记录 acc_0 和 gyr_0。
if ( frame_count != 0 )
{
pre_integrations [ frame_count ] -> push_back ( dt , linear_acceleration , angular_velocity );
//if(solver_flag != NON_LINEAR)
tmp_pre_integration -> push_back ( dt , linear_acceleration , angular_velocity );
dt_buf [ frame_count ]. push_back ( dt );
linear_acceleration_buf [ frame_count ]. push_back ( linear_acceleration );
angular_velocity_buf [ frame_count ]. push_back ( angular_velocity );
int j = frame_count ;
Vector3d un_acc_0 = Rs [ j ] * ( acc_0 - Bas [ j ]) - g ;
Vector3d un_gyr = 0.5 * ( gyr_0 + angular_velocity ) - Bgs [ j ];
Rs [ j ] *= Utility :: deltaQ ( un_gyr * dt ). toRotationMatrix ();
Vector3d un_acc_1 = Rs [ j ] * ( linear_acceleration - Bas [ j ]) - g ;
Vector3d un_acc = 0.5 * ( un_acc_0 + un_acc_1 );
Ps [ j ] += dt * Vs [ j ] + 0.5 * dt * dt * un_acc ;
Vs [ j ] += dt * un_acc ;
}
acc_0 = linear_acceleration ;
gyr_0 = angular_velocity ;
}
void Estimator::processImage()
执行完 processIMU() 后便是执行最重要的 processImage()。以下是 gpt 的回答：
这段代码是一个名为 processImage 的函数，属于 Estimator 类，主要用于处理视觉惯性导航系统（VINS）中的新图像数据。这段代码中，系统接收图像特征数据，并结合惯性测量单元（IMU）的信息，执行关键帧管理、初始化、非线性优化、滑动窗口、故障检测等一系列操作。以下是代码的总结与主要步骤解释：
图像处理和特征点添加 :
函数接收图像特征数据 image 和时间戳 header。
记录图像特征点的数量，并将其添加到特征管理器中进行视差检查，决定是否将当前帧标记为关键帧。
关键帧管理：基于特征的视差决定是否进行边缘化操作。
初始化图像帧和预积分 :
创建 ImageFrame 对象并设置其预积分信息。
将当前帧添加到 all_image_frame 映射中。
初始化或更新预积分对象 tmp_pre_integration。
外参估计 :
如果需要估计外参，系统在足够旋转运动时通过双目视差数据计算外参，并更新外参矩阵 >ric。
初始化阶段 :
单目 + IMU 初始化 : 当使用单目相机和IMU时，系统会在时间窗口填满时尝试初>始化。如果初始化成功，系统进入非线性优化阶段，并执行滑动窗口操作。双目 + IMU 初始化 : 对于双目相机和IMU的初始化，系统会通过PnP方法初始化帧>姿态，进行特征点三角化，并解决陀螺仪偏置问题。仅双目初始化 : 不使用IMU时，系统通过双目视差完成初始化，并在滑动窗口填满后>进入非线性优化。
优化与滑动窗口管理 :
进行非线性优化，并检查和移除异常特征点。
对于未使用多线程的情况，特征跟踪器将进一步移除异常点并预测下一个帧中的特征位置。
检测系统故障，若发生故障，清除系统状态并重新启动。
状态更新 :
更新系统状态，包括关键帧姿态、最后的位姿和状态参数等。
总的来说，这段代码的主要作用是结合图像和IMU数据进行视觉惯性导航系统的初始化、优化和状态更新，从而提高系统的定位精度和鲁棒性。
接下来进行人为总结。
通过检测视差来判断最新到达的图像帧是不是关键帧，如果是，后续的边缘化则将滑窗内最旧的帧边缘化，如果不是，则丢弃第二新的帧。
void Estimator :: processImage ( const map < int , vector < pair < int , Eigen :: Matrix < double , 7 , 1 >>>> & image , const double header )
{
ROS_DEBUG ( "new image coming ------------------------------------------" );
ROS_DEBUG ( "Adding feature points %lu" , image . size ());
if ( f_manager . addFeatureCheckParallax ( frame_count , image , td ))
{
marginalization_flag = MARGIN_OLD ;
//printf("keyframe\n");
}
else
{
marginalization_flag = MARGIN_SECOND_NEW ;
//printf("non-keyframe\n");
}
之后打印一些 debug 信息，如当前帧是否是关键帧、当前帧属于滑窗内第几帧、特征点数量。
创建 ImageFrame imageframe(image, header); 变量，并进行初始化，使其包含了当前帧的特征点、时间戳、是否是关键帧、姿态、位置和其和上一帧之间的 IMU 预积分数据，其定义如下：
class ImageFrame
{
public:
ImageFrame (){};
ImageFrame ( const map < int , vector < pair < int , Eigen :: Matrix < double , 7 , 1 >>>>& _points , double _t ) : t { _t }, is_key_frame { false }
{
points = _points ;
};
map < int , vector < pair < int , Eigen :: Matrix < double , 7 , 1 >> > > points ;
double t ;
Matrix3d R ;
Vector3d T ;
IntegrationBase * pre_integration ;
bool is_key_frame ;
};
将创建好的 imageframe 放入 all_image_frame 的 map 之中，以时间戳为键。然后重新初始化 tmp_pre_integration，为下一关键帧的预积分作准备。
ROS_DEBUG ( "%s" , marginalization_flag ? "Non-keyframe" : "Keyframe" );
ROS_DEBUG ( "Solving %d" , frame_count );
ROS_DEBUG ( "number of feature: %d" , f_manager . getFeatureCount ());
Headers [ frame_count ] = header ;
ImageFrame imageframe ( image , header );
imageframe . pre_integration = tmp_pre_integration ;
all_image_frame . insert ( make_pair ( header , imageframe ));
tmp_pre_integration = new IntegrationBase { acc_0 , gyr_0 , Bas [ frame_count ], Bgs [ frame_count ]};
Eigen::Matrix3d 使用 swap() 方法可能比直接赋值更快，用在 void Estimator::slide() 内。
相机外参在线校准：
if ( ESTIMATE_EXTRINSIC == 2 )
{
ROS_INFO ( "calibrating extrinsic param, rotation movement is needed" );
if ( frame_count != 0 )
{
vector < pair < Vector3d , Vector3d >> corres = f_manager . getCorresponding ( frame_count - 1 , frame_count );
Matrix3d calib_ric ;
if ( initial_ex_rotation . CalibrationExRotation ( corres , pre_integrations [ frame_count ] -> delta_q , calib_ric ))
{
ROS_WARN ( "initial extrinsic rotation calib success" );
ROS_WARN_STREAM ( "initial extrinsic rotation: " << endl << calib_ric );
ric [ 0 ] = calib_ric ;
RIC [ 0 ] = calib_ric ;
ESTIMATE_EXTRINSIC = 1 ;
}
}
}
初始化
外参在线校准完成后，则进行初始化阶段，可以分为三类：
首先讨论单目 + IMU 的情况。如果滑窗没满，则不做任何操作。如果滑窗已满，则判断 外参是否已估计 以及 当前帧距离第一帧是否大于 0.1 s ，若是，则进行 initialStructure()，当初始化成功时，将进入优化与滑窗 的状态，否则只进行滑窗且保持待初始化的状态。
if ( solver_flag == INITIAL )
{
// monocular + IMU initilization
if ( ! STEREO && USE_IMU )
{
if ( frame_count == WINDOW_SIZE )
{
bool result = false ;
if ( ESTIMATE_EXTRINSIC != 2 && ( header - initial_timestamp ) > 0.1 )
{
result = initialStructure ();
initial_timestamp = header ;
}
if ( result )
{
solver_flag = NON_LINEAR ;
optimization ();
slideWindow ();
ROS_INFO ( "Initialization finish!" );
}
else
slideWindow ();
}
}
如果是双目 + IMU 的初始化，则直接使用 pnp 初始化当前帧的 P、R，然后利用该P、R和其他帧（哪一帧呢）三角化当前帧的特征点。
如果滑窗已经满了，则将滑窗内的 P、R 更新给 all_image_frame，之后使用这些位姿来求解陀螺仪的偏置初始值。然后利用陀螺仪偏置的最新初值，来更新滑窗内的各个预积分值。
最后进入优化滑窗的状态。
NOTE ：
在最开始的阶段，所有特征点都没有深度，pnp 函数实际什么也没执行，因此没完成对当前帧（第一帧，frame_count = 0）的位姿初始化
顺序执行到三角化函数，此时 Ps=0 和 Rs=I 都是初始值，因为是双目，因此可利用左右目“瞬间”完成特征点的三角化（“瞬间”是指只使用同一时刻的左右帧即可完成三角化）。
此时 frame_count != WINDOW_SIZE，if 内的代码均不执行
第二帧到来，imu 预积分预测出 Rs[1]、Ps[1] 作为 pnp 初值，并使用跟踪的已三角化的点完成 pnp 位姿初始化
之后用刚计算出的位姿对新检测（未三角化，无深度）的点进行三角化
反复执行
// stereo + IMU initilization
if ( STEREO && USE_IMU )
{
f_manager . initFramePoseByPnP ( frame_count , Ps , Rs , tic , ric );
f_manager . triangulate ( frame_count , Ps , Rs , tic , ric );
if ( frame_count == WINDOW_SIZE )
{
map < double , ImageFrame >:: iterator frame_it ;
int i = 0 ;
for ( frame_it = all_image_frame . begin (); frame_it != all_image_frame . end (); frame_it ++ )
{
frame_it -> second . R = Rs [ i ];
frame_it -> second . T = Ps [ i ];
i ++ ;
}
solveGyroscopeBias ( all_image_frame , Bgs );
for ( int i = 0 ; i <= WINDOW_SIZE ; i ++ )
{
pre_integrations [ i ] -> repropagate ( Vector3d :: Zero (), Bgs [ i ]);
}
solver_flag = NON_LINEAR ;
optimization ();
slideWindow ();
ROS_INFO ( "Initialization finish!" );
}
}
如果是只使用双目，不使用 IMU的情况，到来一帧，无论窗口满或未满，使用 pnp 求解当前帧的初始位姿，然后就直接优化，与前二者窗口满了才优化不同 ，如果窗口满了，才进行滑窗，并进入优化滑窗状态。
if ( STEREO && ! USE_IMU )
{
f_manager . initFramePoseByPnP ( frame_count , Ps , Rs , tic , ric );
f_manager . triangulate ( frame_count , Ps , Rs , tic , ric );
optimization ();
if ( frame_count == WINDOW_SIZE )
{
solver_flag = NON_LINEAR ;
slideWindow ();
ROS_INFO ( "Initialization finish!" );
}
}
在初始化阶段，无论是否为关键帧，均放入滑窗内。如果窗口还没满，则将当前的 P、V、R、B 传给下一帧作为初值，之后会在 processIMU() 累加。
if ( frame_count < WINDOW_SIZE )
{
frame_count ++ ;
int prev_frame = frame_count - 1 ;
Ps [ frame_count ] = Ps [ prev_frame ];
Vs [ frame_count ] = Vs [ prev_frame ];
Rs [ frame_count ] = Rs [ prev_frame ];
Bas [ frame_count ] = Bas [ prev_frame ];
Bgs [ frame_count ] = Bgs [ prev_frame ];
}
滑窗与优化
如果初始化完成，后续该函数将重复执行滑窗与优化。此时 frame_count 将不会再改变，始终等于 WINDOW_SIZE，代码中的窗口大小实际为 WINDOW_SIZE + 1。
首先，如果不使用 IMU，则需要 pnp 求解出新到达帧的初始位姿，否则无需此步骤，因为 IMU 预积分会给出新到达帧的位姿预测。
使用窗口里的历史位姿和刚得到的初始位姿，对还没有深度的点三角化。
optimization();，进行优化，细节见下文
外点去除。先是得到需要去除外点的 id 集合，之后在已有的 fearture_manage 和 feature_tracker 的点集里删去这些点
set < int > removeIndex ;
f_manager . removeOutlier ( removeIndex );
if ( ! MULTIPLE_THREAD )
{
featureTracker . removeOutliers ( removeIndex );
predictPtsInNextFrame ();
}
失效检测，如果满足一些指标，则判定为里程计失效，重置变量，重启系统
slideWindow(); 滑窗，对表示窗口内状态的几个数组操作，边缘化最早的帧或次新帧
删除优化失败的路标点，即以 id 遍历路标点，如果其 solve_flag==2 则删去
最后对一些用于输出的接口变量（latest state）更新
void Estimator::optimization()
该函数被 processImage() 调用，是后端的核心函数。
参数预处理
首先将用 Eigen 表示的状态转换成 double 类型的数组，用于后续的 ceres 优化。
实例化 问题 和 损失函数，即 ceres::Problem problem; 和 ceres::LossFunction *loss_function;。初始化鲁棒优化核函数 Huber 函数。
添加参数块（ParammeterBlock），包括位姿、速度、偏置、外参、时间偏移。步骤如下：
实例化局部参数化变量：ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
向问题添加参数块：problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
对参数可以有一些额外的操作，如冻结某些参数：
// 若未使用 imu，则不对第一帧的位姿作优化
if ( ! USE_IMU )
problem . SetParameterBlockConstant ( para_Pose [ 0 ]);
构建因子图
接下来构建、扩充和更新因子图。
如果存在上次边缘化的信息，则构造并添加新的边缘化因子。
if ( last_marginalization_info && last_marginalization_info -> valid )
{
// construct new marginlization_factor
MarginalizationFactor * marginalization_factor = new MarginalizationFactor ( last_marginalization_info );
problem . AddResidualBlock ( marginalization_factor , NULL ,
last_marginalization_parameter_blocks );
}
如果使用 imu，则添加预积分因子：
if ( USE_IMU )
{
for ( int i = 0 ; i < frame_count ; i ++ )
{
int j = i + 1 ;
if ( pre_integrations [ j ] -> sum_dt > 10.0 )
continue ;
IMUFactor * imu_factor = new IMUFactor ( pre_integrations [ j ]);
problem . AddResidualBlock ( imu_factor , NULL , para_Pose [ i ], para_SpeedBias [ i ], para_Pose [ j ], para_SpeedBias [ j ]);
}
}
之后便是添加相机观测相关的因子。遍历 feature_manager 内的路标点，如果路标点被跟踪帧数小于 4 帧，则不参与优化。对于优化的路标点观测，用到了三种组合：
ProjectionTwoFrameOneCamFactor：pts_i, pts_jProjectionTwoFrameTwoCamFactor：pts_i, pts_j_right（i != j）ProjectionOneFrameTwoCamFactor：pts_i, pts_j_right（i = j）
可视化便是星型网，中心是左目的 start_frame，连接所有其他在 vector<FeaturePerFrame> 的元素。
星型意味着对中心敏感，可改进的点？
设置求解器选项并执行优化
此时已经将涉及的不同类别的因子添加到因子图中，开始对 ceres 求解器作一些设置：
options . linear_solver_type = ceres :: DENSE_SCHUR ;
//options.num_threads = 2;
options . trust_region_strategy_type = ceres :: DOGLEG ;
options . max_num_iterations = NUM_ITERATIONS ;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if ( marginalization_flag == MARGIN_OLD )
options . max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0 ;
else
options . max_solver_time_in_seconds = SOLVER_TIME ;
TicToc t_solver ;
ceres :: Solver :: Summary summary ;
ceres :: Solve ( options , & problem , & summary );
double2vector(); 将求解的结果转变为 Eigen 数据结构。
至此，优化计算的部分已经完成。如果此时窗口内的关键帧还未满，则已经可以退出函数了；但若窗口已满，则需要为下一帧的到来预留位置，因此需要执行边缘化。
if ( frame_count < WINDOW_SIZE )
return ;
TicToc t_whole_marginalization ;
if ( marginalization_flag == MARGIN_OLD )
{
...
边缘化
13 Sep 2024
来自相机的图像被发布，缓存在 img0_buf 和 img1_buf 队列中，新图像放在 back，抽取 front 的图像进行特征追踪。如果是双目，不同步的会被按时间戳先后丢弃，只有双目图像帧的时间戳完全同步才会被使用。
InputImage() 和 InputIMU() 是前端。其实只有 InputImage() 是真正的前端，IMU的预积分其实是放在 processIMU() 里面。
一些重要的变量：
// vins-mono
class FeatureTracker {
public:
vector < int > ids ; // 特征点ID
vector < cv :: Point2f > prev_pts ; // 特征点在前两帧中的坐标
vector < cv :: Point2f > cur_pts ; // 特征点在前一帧中的坐标
vector < cv :: Point2f > forw_pts ; // 特征点在当前帧中的坐标
vector < int > track_cnt ; // 特征点成功追踪帧数
vector < cv :: Point2f > cur_un_pts ; // 特征点归一化坐标系下的坐标
}
// vins-fusion
class FeatureTracker
{
public:
vector < cv :: Point2f > predict_pts ;
vector < cv :: Point2f > predict_pts_debug ;
//特征点在前一帧的坐标、当前左目帧的坐标、当前右目帧的坐标
vector < cv :: Point2f > prev_pts , cur_pts , cur_right_pts ;
// 特征点在前一帧归一化平面中的坐标、当前帧左目、当前帧右目
vector < cv :: Point2f > prev_un_pts , cur_un_pts , cur_un_right_pts ;
vector < cv :: Point2f > pts_velocity , right_pts_velocity ;
vector < int > ids , ids_right ; // 左右目特征点id
vector < int > track_cnt ;
}
reduceVector() 空间复杂度 O(n)，时间复杂度 O（n）
void reduceVector ( vector < cv :: Point2f > & v , vector < uchar > status ) {
int j = 0 ;
for ( int i = 0 ; i < int ( v . size ()); i ++ )
if ( status [ i ])
v [ j ++ ] = v [ i ];
v . resize ( j );
}
光流是基于以下假设下工作的：
在连续的两帧图像之间，目标对象的像素的灰度值不改变。
像素的运动比较微小。
相邻像素具有相似的运动。
cv :: calcOpticalFlowPyrLK ( prev_img ,
cur_img ,
prev_pts ,
cur_pts ,
status ,
err ,
cv :: Size ( 21 , 21 ),
1 ,
cv :: TermCriteria ( cv :: TermCriteria :: COUNT + cv :: TermCriteria :: EPS , 30 , 0.01 ),
cv :: OPTFLOW_USE_INITIAL_FLOW );
现在我们要使用第三条假设，邻域内的所有点都有相似的运动。LucasKanade 法就是利用一个 3x3 邻域中的 9 个点具有相同运动的这一点。这样我们就可以找到\((f_x, f_y, f_t)\)这 9 个点的光流方程，用它们组成一个具有两个未知数 9 个等式的方程组，这是一个约束条件过多的方程组。一个好的解决方法就是使用最小二乘拟合。下面就是求解结果：
\(\begin{bmatrix} u \ v \end{bmatrix} = \begin{bmatrix} \sum_{i}{f_{x_i}}^2 & \sum_{i}{f_{x_i} f_{y_i} } \ \sum_{i}{f_{x_i} f_{y_i}} & \sum_{i}{f_{y_i}}^2 \end{bmatrix}^{-1} \begin{bmatrix} - \sum_{i}{f_{x_i} f_{t_i}} \ - \sum_{i}{f_{y_i} f_{t_i}} \end{bmatrix}\)
你会发现上边的逆矩阵与 Harris 角点检测器非常相似，这说明角点很适合被用来做跟踪） 从使用者的角度来看，想法很简单，我们去跟踪一些点，然后我们就会获得这些点的光流向量。但是还有一些问题。直到现在我们处理的都是很小的运动。如果有大的运动怎么办呢？图像金字塔。当我们进入金字塔时，小运动被移除，大运动变成小运动。因此，通过在那里应用Lucas-Kanade，我们就会得到尺度空间上的光流。6.2. 光流 zhihu -（三十八）稀疏光流—-KLT
金字塔Lucas-Kanade跟踪方法是：在图像金字塔的最高层计算光流，用得到的运动估计结果作为下一层金字塔的起始点，重复这个过程直到到达金字塔的最底层。这样就将不满足运动的假设可能性降到最小从而实现对更快和更长的运动的跟踪。
FeatureTracker::trackImage() 解读
map < int , vector < pair < int , Eigen :: Matrix < double , 7 , 1 >>>>
FeatureTracker :: trackImage ( double _cur_time , const cv :: Mat & _img , const cv :: Mat & _img1 )
输入是当前帧的时间、左目图像帧、右目图像帧
函数开头定义了一些局部变量，并清空用于存储当前帧特征点像素坐标的 vector：
TicToc t_r ;
cur_time = _cur_time ;
cur_img = _img ;
row = cur_img . rows ;
col = cur_img . cols ;
cv :: Mat rightImg = _img1 ;
cur_pts . clear ();
首先判断上一帧的特征点个数是否大于 0，如果大于 0，则进行特征点的跟踪，否则什么也不做。
若进行特征点的跟踪，首先判断是否使用 gpu 加速，之后无论是否使用 gpu 加速，均判断是否使用预测特征点和是否使用反向光流跟踪
如果使用了预测，则利用预测的先验值来帮助LK光流跟踪，如果没有使用预测，则使用普通的三层金字塔LK光流跟踪。
如果使用了反向光流跟踪，正反向都能跟踪上且反向跟踪的点和原始点的误差小于 0.5 像素的点会被保留
而对于使用 gpu 加速的情况，需要将对应的图像矩阵变量和光流跟踪函数替换成对应的 cuda 版本：
cv :: cuda :: GpuMat prev_gpu_img ( prev_img );
cv :: cuda :: GpuMat cur_gpu_img ( cur_img );
cv :: cuda :: GpuMat prev_gpu_pts ( prev_pts );
cv :: cuda :: GpuMat cur_gpu_pts ( cur_pts );
cv :: cuda :: GpuMat gpu_status ;
// flow forward gpu version
cv :: Ptr < cv :: cuda :: SparsePyrLKOpticalFlow > d_pyrLK_sparse = cv :: cuda :: SparsePyrLKOpticalFlow :: create (
cv :: Size ( 21 , 21 ), 3 , 30 , false );
d_pyrLK_sparse -> calc ( prev_gpu_img , cur_gpu_img , prev_gpu_pts , cur_gpu_pts , gpu_status );
vector < cv :: Point2f > tmp1_cur_pts ( cur_gpu_pts . cols );
cur_gpu_pts . download ( tmp1_cur_pts );
cur_pts = tmp1_cur_pts ;
vector < uchar > tmp1_status ( gpu_status . cols );
gpu_status . download ( tmp1_status );
status = tmp1_status ;
之后对剩下的点作最后一层筛选，即判断当前帧的特征点是否在边界内，不在的将 state 设为 0：
for ( int i = 0 ; i < int ( cur_pts . size ()); i ++ )
if ( status [ i ] && ! inBorder ( cur_pts [ i ]))
status [ i ] = 0 ;
reduceVector ( prev_pts , status );
reduceVector ( cur_pts , status );
reduceVector ( ids , status );
reduceVector ( track_cnt , status );
然后将最终跟踪下来的特征点的 cnt 加一：
for ( auto & n : track_cnt )
n ++ ;
void FeatureTracker::setMask() 内将 cnt、pts、ids 合并成 vector，之后按 cnt 的大小进行排序，然后利用 mask 实现均匀化，也就是反复仿佛雨滴首先滴在 cnt 大的点上，其他点如果在这个雨滴内，就不会被放入 vector 里了，从而会删去一些点，这样也能够保证 cnt 大的特征点优先保留下来：
void FeatureTracker :: setMask ()
{
mask = cv :: Mat ( row , col , CV_8UC1 , cv :: Scalar ( 255 ));
// prefer to keep features that are tracked for long time
vector < pair < int , pair < cv :: Point2f , int >>> cnt_pts_id ;
for ( unsigned int i = 0 ; i < cur_pts . size (); i ++ )
cnt_pts_id . push_back ( make_pair ( track_cnt [ i ], make_pair ( cur_pts [ i ], ids [ i ])));
sort ( cnt_pts_id . begin (), cnt_pts_id . end (), []( const pair < int , pair < cv :: Point2f , int >> & a , const pair < int , pair < cv :: Point2f , int >> & b )
{
return a . first > b . first ;
});
cur_pts . clear ();
ids . clear ();
track_cnt . clear ();
for ( auto & it : cnt_pts_id )
{
if ( mask . at < uchar > ( it . second . first ) == 255 )
{
cur_pts . push_back ( it . second . first );
ids . push_back ( it . second . second );
track_cnt . push_back ( it . first );
cv :: circle ( mask , it . second . first , MIN_DIST , 0 , - 1 );
}
}
}
将跟踪到的点均匀化并设置 mask 后，如果当前特征点个数小于 最大跟踪数目 ，则检测新的 goodfeature，检测也可以分成使用 gpu 加速和不使用 gpu 加速的。检测出新的点后，坐标都只是先存在 vector<cv::Point2f> n_pts 里，需要将其添加到 cur_pts、ids、track_cnt 里，ids就是总的ids记录上累加 1，track_cnt 均等于 1：
NOTE: 若要加协调器，分配的最大检测特征点数目就对应于此
int n_max_cnt = MAX_CNT - static_cast < int > ( cur_pts . size ());
if ( ! USE_GPU )
{
if ( n_max_cnt > 0 )
{
TicToc t_t ;
if ( mask . empty ())
cout << "mask is empty " << endl ;
if ( mask . type () != CV_8UC1 )
cout << "mask type wrong " << endl ;
cv :: goodFeaturesToTrack ( cur_img , n_pts , MAX_CNT - cur_pts . size (), 0.01 , MIN_DIST , mask );
// printf("good feature to track costs: %fms\n", t_t.toc());
std :: cout << "n_pts size: " << n_pts . size () << std :: endl ;
}
else
n_pts . clear ();
// sum_n += n_pts.size();
// printf("total point from non-gpu: %d\n",sum_n);
}
// ROS_DEBUG("detect feature costs: %fms", t_t.toc());
// printf("good feature to track costs: %fms\n", t_t.toc());
else
{
if ( n_max_cnt > 0 )
{
if ( mask . empty ())
cout << "mask is empty " << endl ;
if ( mask . type () != CV_8UC1 )
cout << "mask type wrong " << endl ;
TicToc t_g ;
cv :: cuda :: GpuMat cur_gpu_img ( cur_img );
cv :: cuda :: GpuMat d_prevPts ;
TicToc t_gg ;
cv :: cuda :: GpuMat gpu_mask ( mask );
// printf("gpumat cost: %fms\n",t_gg.toc());
cv :: Ptr < cv :: cuda :: CornersDetector > detector = cv :: cuda :: createGoodFeaturesToTrackDetector ( cur_gpu_img . type (), MAX_CNT - cur_pts . size (), 0.01 , MIN_DIST );
// cout << "new gpu points: "<< MAX_CNT - cur_pts.size()<<endl;
detector -> detect ( cur_gpu_img , d_prevPts , gpu_mask );
// std::cout << "d_prevPts size: "<< d_prevPts.size()<<std::endl;
if ( ! d_prevPts . empty ())
n_pts = cv :: Mat_ < cv :: Point2f > ( cv :: Mat ( d_prevPts ));
else
n_pts . clear ();
// sum_n += n_pts.size();
// printf("total point from gpu: %d\n",sum_n);
// printf("gpu good feature to track cost: %fms\n", t_g.toc());
}
else
n_pts . clear ();
}
ROS_DEBUG ( "add feature begins" );
TicToc t_a ;
addPoints ();
// ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
// printf("selectFeature costs: %fms\n", t_a.toc());
}
void FeatureTracker :: addPoints ()
{
for ( auto & p : n_pts )
{
cur_pts . push_back ( p );
ids . push_back ( n_id ++ );
track_cnt . push_back ( 1 );
}
}
相机的参数传递流：
rosNodeTest.cpp
string config_file = argv [ 1 ];
printf ( "config_file: %s \n " , argv [ 1 ]);
readParameters ( config_file );
estimator . setParameter ();
parameters.cpp
int pn = config_file . find_last_of ( '/' );
std :: string configPath = config_file . substr ( 0 , pn );
std :: string cam0Calib ;
fsSettings [ "cam0_calib" ] >> cam0Calib ;
std :: string cam0Path = configPath + "/" + cam0Calib ;
CAM_NAMES . push_back ( cam0Path );
estimator.cpp
void Estimator :: setParameter ()
{
···
featureTracker . readIntrinsicParameter ( CAM_NAMES );
···
}
feature_tracker.cpp
void FeatureTracker :: readIntrinsicParameter ( const vector < string > & calib_file )
{
for ( size_t i = 0 ; i < calib_file . size (); i ++ )
{
ROS_INFO ( "reading paramerter of camera %s" , calib_file [ i ]. c_str ());
camodocal :: CameraPtr camera = CameraFactory :: instance () -> generateCameraFromYamlFile ( calib_file [ i ]);
m_camera . push_back ( camera );
}
if ( calib_file . size () == 2 )
stereo_cam = 1 ;
}
对剩余的点去畸变，抬升到归一化平面，并计算在该平面上的速度：
cur_un_pts = undistortedPts ( cur_pts , m_camera [ 0 ]);
pts_velocity = ptsVelocity ( ids , cur_un_pts , cur_un_pts_map , prev_un_pts_map );
若有右目，则将到此为止的左目特征点跟踪到右目，并且 id 和坐标单独记录，不记录跟踪帧数，因为右目的特征点由已均匀化的左目特征点跟踪而来，因此没必要均匀化了。
最后则是将筛选后幸存下来的特征点存储在 featureFrame 里。
map < int , vector < pair < int , Eigen :: Matrix < double , 7 , 1 >>>> featureFrame ;
for ( size_t i = 0 ; i < ids . size (); i ++ )
{
int feature_id = ids [ i ];
double x , y , z ;
x = cur_un_pts [ i ]. x ;
y = cur_un_pts [ i ]. y ;
z = 1 ;
double p_u , p_v ;
p_u = cur_pts [ i ]. x ;
p_v = cur_pts [ i ]. y ;
int camera_id = 0 ;
double velocity_x , velocity_y ;
velocity_x = pts_velocity [ i ]. x ;
velocity_y = pts_velocity [ i ]. y ;
Eigen :: Matrix < double , 7 , 1 > xyz_uv_velocity ;
xyz_uv_velocity << x , y , z , p_u , p_v , velocity_x , velocity_y ;
featureFrame [ feature_id ]. emplace_back ( camera_id , xyz_uv_velocity );
}
if ( ! _img1 . empty () && stereo_cam )
{
for ( size_t i = 0 ; i < ids_right . size (); i ++ )
{
int feature_id = ids_right [ i ];
double x , y , z ;
x = cur_un_right_pts [ i ]. x ;
y = cur_un_right_pts [ i ]. y ;
z = 1 ;
double p_u , p_v ;
p_u = cur_right_pts [ i ]. x ;
p_v = cur_right_pts [ i ]. y ;
int camera_id = 1 ;
double velocity_x , velocity_y ;
velocity_x = right_pts_velocity [ i ]. x ;
velocity_y = right_pts_velocity [ i ]. y ;
Eigen :: Matrix < double , 7 , 1 > xyz_uv_velocity ;
xyz_uv_velocity << x , y , z , p_u , p_v , velocity_x , velocity_y ;
featureFrame [ feature_id ]. emplace_back ( camera_id , xyz_uv_velocity );
}
}
思考
如果使用不同的特征提取和匹配方法，也可依照上述操作，每个相机的不同帧（我们希望对这些帧的特征点进行提取和匹配）均分配多个 vector，分别记录 id、图像坐标、归一化坐标、归一化坐标速度等信息，最终利用匹配结果去合并（修改）某些id，最后存入 map 中，用于后续的管理。