0. 简介
上一节中,我们过完了VIO中的状态预测以及特征点跟踪部分。此时我们已经拿到了光流的特征点信息,而这部分越来越接近我们想要去讲的帧到帧的VIO部分了。这一节,我们将围绕着VIO部分来进行讲解
1. PNP误差更新
我们从之前的博客《经典文献阅读之—R3LIVE》了解到,帧到帧的VIO是将三维地图点投影到上一个图像帧获取二维座标然后通过LK光流法获取到在当前帧的二维坐标,然后可以通过ESIKF计算误差更新状态(计算PNP,更新ESIKF)
而ESIKF更新VIO是通过最小化PNP误差更新的,对应了下面这部分代码
// Pnp求解,然后更新img_pose
if ( op_track.remove_outlier_using_ransac_pnp( img_pose ) == 0 )
{
cout << ANSI_COLOR_RED_BOLD << "****** Remove_outlier_using_ransac_pnp error*****" << ANSI_COLOR_RESET << endl;
}
我们知道PnP(Perspective-n-Point)是求解3D到2D点的对应方法。它描述了当知道n个3D空间点及其位置,如何估计相机的位姿。详细推导看上面链接即可,这里就不过多展开了。
对于帧间追踪,假设上一帧追踪到m个地图点\mathcal{P}=\left{\mathbf{P}_{1}, \ldots, \mathbf{P}_{m}\right},它们在上一帧图像平面上的投影点也是知道的,通过LK光流可以得到这些投影点在当前帧上的估计位置。随后构建重投影误差,通过ESIKF的方式更新状态{\mathbf{x}}_{k}。下面我们来看一下代码中PNP是如何求出误差的:
if ( 1 )
{
std::vector< int > status;
try
{
cv::solvePnPRansac( pt_3d_vec, pt_2d_vec, m_intrinsic, cv::Mat(), r_vec, t_vec, false, 200, 1.5, 0.99,
status ); // SOLVEPNP_ITERATIVE
}
catch ( cv::Exception &e )
{
scope_color( ANSI_COLOR_RED_BOLD );
cout << "Catching a cv exception: " << e.msg << endl;
return 0;
}
if ( if_remove_ourlier )
{
// Remove outlier
m_map_rgb_pts_in_last_frame_pos.clear();
m_map_rgb_pts_in_current_frame_pos.clear();
for ( unsigned int i = 0; i < status.size(); i++ )
{
int inlier_idx = status[ i ];
{
m_map_rgb_pts_in_last_frame_pos[ map_ptr_vec[ inlier_idx ] ] = pt_2d_vec[ inlier_idx ];
m_map_rgb_pts_in_current_frame_pos[ map_ptr_vec[ inlier_idx ] ] = pt_2d_vec[ inlier_idx ];
}
}
}
update_last_tracking_vector_and_ids();
}
文中的公式3,4代表了投影误差计算,公式3中^Cp_s代表了在相机坐标系下的s点。I_{k-1}代表了上一时刻的图像帧,以及他们在\mathbf{I}_{k-1}中会存在有\left{\mathcal{\rho}_{1_k}, \ldots, \mathcal{\rho}_{m_k}\right},投影误差对应了公式4。
其中公式4中的\pi(^Cp_s,\check{x}_k)\in\mathbb{R}^2项是公式5,其中第一项是针孔相机模型的投影函数,第二项为在线时间校正因子(Online-temporal correction factor)。
相关的符号具体含义可以参考下面的表格
2. 帧到帧VIO
下面就是完成帧到帧的VIO的部分了
///vio_esikf,依据重投影误差,更新优化状态量state_out
res_esikf = vio_esikf( state_out, op_track );
g_cost_time_logger.record( tim, "Vio_f2f" );
tim.tic( "Vio_f2m" );
对应的函数为vio_esikf
文中提到,测量噪声包含两个来源,一个是像素追踪误差,另一个是地图点定位误差:
其中^G\mathbf{p}_{s}^{\mathrm{gt}}和\rho_{s_{k}}^{\mathrm{gt}}分别为^G\mathbf{p}_{s}和\rho_{s_{k}}的真值。然后根据公式4将残差一阶泰勒展开
// 遍历当前帧跟踪到的地图点
for ( auto it = op_track.m_map_rgb_pts_in_last_frame_pos.begin(); it != op_track.m_map_rgb_pts_in_last_frame_pos.end(); it++ )
{
// 取地图坐标系中的地图点坐标
pt_3d_w = ( ( RGB_pts * ) it->first )->get_pos();
// 跟踪像素点的速度
pt_img_vel = ( ( RGB_pts * ) it->first )->m_img_vel;
// 该地图点对应于上一帧的像素点坐标
pt_img_measure = vec_2( it->second.x, it->second.y );
// 将地图点从世界坐标系变换到当前帧相机坐标系
pt_3d_cam = R_w2c * pt_3d_w + t_w2c;
// 利用相机模型以及估计的相机-imu时间偏差导致的增量,得到地图点在当前帧图像的投影像素坐标 pt_img_proj
pt_img_proj = vec_2( fx * pt_3d_cam( 0 ) / pt_3d_cam( 2 ) + cx, fy * pt_3d_cam( 1 ) / pt_3d_cam( 2 ) + cy ) + time_td * pt_img_vel;
// 计算重投影误差,这里计算的是误差的大小,不用于迭代求解,只是用来确定huber核函数
double repro_err = ( pt_img_proj - pt_img_measure ).norm();
double huber_loss_scale = get_huber_loss_scale( repro_err );//huber 1.0
pt_idx++;
acc_reprojection_error += repro_err;
// if (iter_count == 0 || ((repro_err - last_reprojection_error_vec[pt_idx]) < 1.5))
if ( iter_count == 0 || ( ( repro_err - last_avr_repro_err * 5.0 ) < 0 ) )
{
last_reprojection_error_vec[ pt_idx ] = repro_err;
}
else
{
last_reprojection_error_vec[ pt_idx ] = repro_err;
}
avail_pt_count++;
其中
根据这个一阶展开,可以和IMU传播的先验分布组合,获得当前估计位姿的MAP
其中|x|^2_{\sum}=x^T\sum^{-1}x是带有协方差\sum的马氏距离平方,\hat{x}_k是IMU传播的状态估计,\mathcal{H}是从\hat{x}_k切平面投影到\check{x}_k切平面的状态误差的雅克比矩阵,其中公式(12)的详细推导可以看R2LIVE的E小节。
// Appendix E of r2live_Supplementary_material.
// https://github.com/hku-mars/r2live/blob/master/supply/r2live_Supplementary_material.pdf
// 像素对相机点求雅可比
mat_pre << fx / pt_3d_cam( 2 ), 0, -fx * pt_3d_cam( 0 ) / pt_3d_cam( 2 ), 0, fy / pt_3d_cam( 2 ), -fy * pt_3d_cam( 1 ) / pt_3d_cam( 2 );
pt_hat = Sophus::SO3d::hat( ( R_imu.transpose() * ( pt_3d_w - t_imu ) ) );//IMU系下地图点 p(IMU)^ = R(IMU <-- W) * ( p(W) - p(IMU) )
mat_A = state_iter.rot_ext_i2c.transpose() * pt_hat;//3 * 3, R(C <-- IMU) * p(imu)^
mat_B = -state_iter.rot_ext_i2c.transpose() * ( R_imu.transpose() );// - R(C <--IMU) * R(IMU <-- W)
mat_C = Sophus::SO3d::hat( pt_3d_cam );// p(C)^
mat_D = -state_iter.rot_ext_i2c.transpose();// - R(C <-- IMU)
meas_vec.block( pt_idx * 2, 0, 2, 1 ) = ( pt_img_proj - pt_img_measure ) * huber_loss_scale / img_res_scale;//观测向量填充
H_mat.block( pt_idx * 2, 0, 2, 3 ) = mat_pre * mat_A * huber_loss_scale;// H, 1-2行,前3列, 对R(IMU)雅可比
H_mat.block( pt_idx * 2, 3, 2, 3 ) = mat_pre * mat_B * huber_loss_scale;// H, 1-2行,4-6列,对P(IMU)雅可比
if ( DIM_OF_STATES > 24 )
{
// Estimate time td. 对时间差的导数
H_mat.block( pt_idx * 2, 24, 2, 1 ) = pt_img_vel * huber_loss_scale;// H,1-2行, 25-26列,对像素速度雅可比
// H_mat(pt_idx * 2, 24) = pt_img_vel(0) * huber_loss_scale;
// H_mat(pt_idx * 2 + 1, 24) = pt_img_vel(1) * huber_loss_scale;
}
if ( m_if_estimate_i2c_extrinsic )
{
H_mat.block( pt_idx * 2, 18, 2, 3 ) = mat_pre * mat_C * huber_loss_scale;//H ,1-2行,19-21列,对外参R(IMU<--C)雅可比
H_mat.block( pt_idx * 2, 21, 2, 3 ) = mat_pre * mat_D * huber_loss_scale;//H ,1-2行,22-24列,对外参t(IMU<--C)雅可比
}
if ( m_if_estimate_intrinsic )
{
H_mat( pt_idx * 2, 25 ) = pt_3d_cam( 0 ) / pt_3d_cam( 2 ) * huber_loss_scale;//H,1行,26列,对内参fx雅可比
H_mat( pt_idx * 2 + 1, 26 ) = pt_3d_cam( 1 ) / pt_3d_cam( 2 ) * huber_loss_scale;//H,2行,27列,对内参fy雅可比
H_mat( pt_idx * 2, 27 ) = 1 * huber_loss_scale;//H,1行,28列,对内参cx雅可比
H_mat( pt_idx * 2 + 1, 28 ) = 1 * huber_loss_scale;//H,2行,29列,对内参cy雅可比
}
}
给出卡尔曼滤波的相关定义:
卡尔曼增益可以定义为:
状态更新为:
并且根据FAST-LIO2的公式推导,这样的迭代更新过程是等价于高斯牛顿优化的,对应的代码如下。
H_mat = H_mat / img_res_scale;
acc_reprojection_error /= total_pt_size;
last_avr_repro_err = acc_reprojection_error;
if ( avail_pt_count < minimum_iteration_pts )
{
break;
}
H_mat_spa = H_mat.sparseView();
Eigen::SparseMatrix< double > Hsub_T_temp_mat = H_mat_spa.transpose();
vec_spa = ( state_iter - state_in ).sparseView();
H_T_H_spa = Hsub_T_temp_mat * H_mat_spa;
// Notice that we have combine some matrix using () in order to boost the matrix multiplication.
//(H^T * H + P^-1)^-1
Eigen::SparseMatrix< double > temp_inv_mat =
( ( H_T_H_spa.toDense() + eigen_mat< -1, -1 >( state_in.cov * m_cam_measurement_weight ).inverse() ).inverse() ).sparseView();
KH_spa = temp_inv_mat * ( Hsub_T_temp_mat * H_mat_spa );//K * H
// (H^T * H + P^-1)^-1 * ( H^T * (-Z) ) - ( I - K * H) * (X_k - X_0)
// delta_error_X = X_k+1 - X_k = -K * Z - (I - K * H) * (X_k - X_0)
solution = ( temp_inv_mat * ( Hsub_T_temp_mat * ( ( -1 * meas_vec.sparseView() ) ) ) - ( I_STATE_spa - KH_spa ) * vec_spa ).toDense();
state_iter = state_iter + solution;
if ( fabs( acc_reprojection_error - last_repro_err ) < 0.01 )
{
break;
}
last_repro_err = acc_reprojection_error;
3. 帧到地图VIO
帧到地图的光度误差更新,这部分代码在帧与帧VIO后,基本上和帧到帧的VIO一样。
在帧间VIO更新后,我们得到了一个更新好的估计状态\check{x}_k,然后我们通过最小跟踪点的光度误差来求解帧到地图,以降低漂移。
///以ESIKF形式,依据rgb误差,更新优化状态量state_out
res_photometric = vio_photometric( state_out, op_track, img_pose );
g_cost_time_logger.record( tim, "Vio_f2m" );
g_lio_state = state_out;//更新当前body的状态量
print_dash_board();
set_image_pose( img_pose, state_out );//更新image状态量
误差直接用帧到三维地图点的光度误差,公式如下,其中c_s是保存在全局地图中点的颜色,\gamma_s是当前帧图像\mathbf{I}_k中观测到的颜色。
为了获得观测颜色以及协方差矩阵\sum_{n_{\gamma_s}},文中预测当前帧图像\mathbf{I}_k上的点坐标\tilde{\rho}_{s_k}= \pi(^Cp_s,\check{x}_k),然后线性差值计算得到相邻像素的RGB颜色。同时两个参数有各自对应的测量噪声。
与前面一样,根据前面的19-21,完成一阶泰勒展开:
///遍历所有地图点,计算雅可比矩阵和观测向量
for ( auto it = op_track.m_map_rgb_pts_in_last_frame_pos.begin(); it != op_track.m_map_rgb_pts_in_last_frame_pos.end(); it++ )
{
if ( ( ( RGB_pts * ) it->first )->m_N_rgb < 3 ) //没有rgb跳过
{
continue;
}
pt_idx++;
pt_3d_w = ( ( RGB_pts * ) it->first )->get_pos();
pt_img_vel = ( ( RGB_pts * ) it->first )->m_img_vel;
pt_img_measure = vec_2( it->second.x, it->second.y );
pt_3d_cam = R_w2c * pt_3d_w + t_w2c; //3D地图点转换到相机系下
pt_img_proj = vec_2( fx * pt_3d_cam( 0 ) / pt_3d_cam( 2 ) + cx, fy * pt_3d_cam( 1 ) / pt_3d_cam( 2 ) + cy ) + time_td * pt_img_vel;//投影到像素坐标
vec_3 pt_rgb = ( ( RGB_pts * ) it->first )->get_rgb(); //地图点rgb
mat_3_3 pt_rgb_info = mat_3_3::Zero();
mat_3_3 pt_rgb_cov = ( ( RGB_pts * ) it->first )->get_rgb_cov();//地图点rgb对角协方差矩阵
for ( int i = 0; i < 3; i++ )
{
pt_rgb_info( i, i ) = 1.0 / pt_rgb_cov( i, i ) / 255; //info_rgb(map) = 1 / (cov_rgb(map) * 255)
R_mat_inv( pt_idx * err_size + i, pt_idx * err_size + i ) = pt_rgb_info( i, i );//斜对角块 R_i = 1 / (cov_pt_i_cov * 255)
// R_mat_inv( pt_idx * err_size + i, pt_idx * err_size + i ) = 1.0;
}
vec_3 obs_rgb_dx, obs_rgb_dy; //影像中x、y方向上的rgb差值
//投影点从影像插值获取rgb,计算x、y方向上的rgb差值
vec_3 obs_rgb = image->get_rgb( pt_img_proj( 0 ), pt_img_proj( 1 ), 0, &obs_rgb_dx, &obs_rgb_dy );
vec_3 photometric_err_vec = ( obs_rgb - pt_rgb ); //rgb误差: 图像对应点rgb - 地图点rgb
double huber_loss_scale = get_huber_loss_scale( photometric_err_vec.norm() ); //huber 1.0
photometric_err_vec *= huber_loss_scale;
double photometric_err = photometric_err_vec.transpose() * pt_rgb_info * photometric_err_vec; //影像误差 e^T * info_rgb(map) * e
acc_photometric_error += photometric_err;
last_reprojection_error_vec[ pt_idx ] = photometric_err;//影像误差向量索引i填充
然后下面的图片代表了不同成分的含义
avail_pt_count++;
// 像素坐标对相机坐标求导 ,-z^2????
mat_pre << fx / pt_3d_cam( 2 ), 0, -fx * pt_3d_cam( 0 ) / pt_3d_cam( 2 ), 0, fy / pt_3d_cam( 2 ), -fy * pt_3d_cam( 1 ) / pt_3d_cam( 2 );
mat_d_pho_d_img = mat_photometric * mat_pre;
//jacobian与vio_esikf相同
pt_hat = Sophus::SO3d::hat( ( R_imu.transpose() * ( pt_3d_w - t_imu ) ) ); //IMU系下地图点 p(IMU)^ = R(IMU <-- W) * ( p(W) - p(IMU) )
mat_A = state_iter.rot_ext_i2c.transpose() * pt_hat; //3 * 3, R(C <-- IMU) * p(imu)^
mat_B = -state_iter.rot_ext_i2c.transpose() * ( R_imu.transpose() ); // - R(C <--IMU) * R(IMU <-- W)
mat_C = Sophus::SO3d::hat( pt_3d_cam );// p(C)^
mat_D = -state_iter.rot_ext_i2c.transpose();// - R(C <-- IMU)
meas_vec.block( pt_idx * 3, 0, 3, 1 ) = photometric_err_vec ; //观测向量填充
H_mat.block( pt_idx * 3, 0, 3, 3 ) = mat_d_pho_d_img * mat_A * huber_loss_scale;
H_mat.block( pt_idx * 3, 3, 3, 3 ) = mat_d_pho_d_img * mat_B * huber_loss_scale;
if ( 1 )
{
if ( m_if_estimate_i2c_extrinsic )
{
H_mat.block( pt_idx * 3, 18, 3, 3 ) = mat_d_pho_d_img * mat_C * huber_loss_scale; //外参雅可比
H_mat.block( pt_idx * 3, 21, 3, 3 ) = mat_d_pho_d_img * mat_D * huber_loss_scale;
}
}
}
R_mat_inv_spa = R_mat_inv.sparseView();
last_avr_repro_err = acc_photometric_error;
if ( avail_pt_count < minimum_iteration_pts )
{
break;
}
根据公式22,就构成了\delta{\check{x}}_k的另一个观测分布,它与IMU传播的先验分布相结合,得到了\delta{\check{x}}_k的最大后验估计,这部分内容和上面类似
然后下面依旧是卡尔曼定义,增益,及更新,和13-16公式基本相同
// Esikf
tim.tic( "Iter" );
if ( if_esikf )
{
H_mat_spa = H_mat.sparseView();
Eigen::SparseMatrix< double > Hsub_T_temp_mat = H_mat_spa.transpose();
vec_spa = ( state_iter - state_in ).sparseView();//误差
H_T_H_spa = Hsub_T_temp_mat * R_mat_inv_spa * H_mat_spa; // H^T * R^-1 * H
//(H^T * R^-1 * H + P^-1)^-1
Eigen::SparseMatrix< double > temp_inv_mat =
( H_T_H_spa.toDense() + ( state_in.cov * m_cam_measurement_weight ).inverse() ).inverse().sparseView();
Eigen::SparseMatrix< double > Ht_R_inv = ( Hsub_T_temp_mat * R_mat_inv_spa ); //H^T * R^-1
KH_spa = Ht_R_inv * H_mat_spa; //H^T * R^-1 * H
//((H^T * R^-1 * H + P^-1)^-1 * (H^T * R^-1 * (-Z) - (I - (H^T * R^-1 * H + P^-1)^-1 * H^T * R^-1 * H ) * (X_k - X_0)
// delta_error_X = X_k+1 - X_k = K * (-Z) - ( I - K * H ) * (X_k - X_0)
solution =
( temp_inv_mat * ( Ht_R_inv * ( ( -1 * meas_vec.sparseView() ) ) ) - ( I_STATE_spa - temp_inv_mat * KH_spa ) * vec_spa ).toDense();
}
state_iter = state_iter + solution;
// state_iter.cov = ((I_STATE_spa - KH_spa) * state_iter.cov.sparseView()).toDense();
if ( ( acc_photometric_error / total_pt_size ) < 10 ) // By experience.
{
break;
}
if ( fabs( acc_photometric_error - last_repro_err ) < 0.01 )
{
break;
}
last_repro_err = acc_photometric_error;
然后执行类似公式17,18的状态更新,将帧到地图的VIO ESIKF更新知道收敛,然后用收敛的值去渲染地图纹理并作为下一帧imu传播的起点使用。
if ( if_esikf && avail_pt_count >= minimum_iteration_pts )
{
state_iter.cov = ( ( I_STATE_spa - KH_spa ) * state_iter.cov.sparseView() ).toDense(); //更新协方差矩阵
}
state_iter.td_ext_i2c += state_iter.td_ext_i2c_delta;
state_iter.td_ext_i2c_delta = 0;
state_in = state_iter;
4. 地图点颜色更新
还有一步就是给全局地图渲染颜色(即地图点颜色更新):同样以某个地图点为例,若它被当前帧观测到,通过优化后的位姿以及内参投影到当前帧的位置可能非整数点(这会造成该点没有对应的观测颜色),此时通过邻域插值得到的该观测颜色,之后通过贝叶斯更新过程将这两个颜色进行“融合”。(最后一步是更新VIO追踪的地图点,此时会删除一些光度误差较大的点,并将地图点投影到当前帧新增追踪到的点),下面就是对应的代码部分
if ( 1 )
{
tim.tic( "Render" );
// m_map_rgb_pts.render_pts_in_voxels(img_pose, m_last_added_rgb_pts_vec);
if ( 1 ) // Using multiple threads for rendering
{
m_map_rgb_pts.m_if_get_all_pts_in_boxes_using_mp = 0;
// m_map_rgb_pts.render_pts_in_voxels_mp(img_pose, &m_map_rgb_pts.m_rgb_pts_in_recent_visited_voxels,
// img_pose->m_timestamp);
//更新activated voxel内点的rgb、到相机距离、时间
m_render_thread = std::make_shared< std::shared_future< void > >( m_thread_pool_ptr->commit_task(
render_pts_in_voxels_mp, img_pose, &m_map_rgb_pts.m_voxels_recent_visited, img_pose->m_timestamp ) );
}
else
{
m_map_rgb_pts.m_if_get_all_pts_in_boxes_using_mp = 0;
// m_map_rgb_pts.render_pts_in_voxels( img_pose, m_map_rgb_pts.m_rgb_pts_in_recent_visited_voxels,
// img_pose->m_timestamp );
}
m_map_rgb_pts.m_last_updated_frame_idx = img_pose->m_frame_idx;
g_cost_time_logger.record( tim, "Render" );
tim.tic( "Mvs_record" );
if ( m_if_record_mvs )
{
// m_mvs_recorder.insert_image_and_pts( img_pose, m_map_rgb_pts.m_voxels_recent_visited );
m_mvs_recorder.insert_image_and_pts( img_pose, m_map_rgb_pts.m_pts_last_hitted );//插入点
}
g_cost_time_logger.record( tim, "Mvs_record" );
}
// ANCHOR - render point cloud
dump_lio_state_to_log( m_lio_state_fp );
m_mutex_lio_process.unlock();
// cout << "Solve image pose cost " << tim.toc("Solve_pose") << endl;
m_map_rgb_pts.update_pose_for_projection( img_pose, -0.4 );//更新一些相机参数
//更新和添加新的追踪点、索引等
op_track.update_and_append_track_pts( img_pose, m_map_rgb_pts, m_track_windows_size / m_vio_scale_factor, 1000000 );
g_cost_time_logger.record( tim, "Frame" );
double frame_cost = tim.toc( "Frame" );
g_image_vec.push_back( img_pose );
frame_cost_time_vec.push_back( frame_cost );//耗时
if ( g_image_vec.size() > 10 )//防止内存占用过多
{
g_image_vec.pop_front();
frame_cost_time_vec.pop_front();
}
tim.tic( "Pub" );
double display_cost_time = std::accumulate( frame_cost_time_vec.begin(), frame_cost_time_vec.end(), 0.0 ) / frame_cost_time_vec.size();
g_vio_frame_cost_time = display_cost_time; //Frame平均耗时
// publish_render_pts( m_pub_render_rgb_pts, m_map_rgb_pts );
publish_camera_odom( img_pose, message_time );
// publish_track_img( op_track.m_debug_track_img, display_cost_time );
publish_track_img( img_pose->m_raw_img, display_cost_time );
if ( m_if_pub_raw_img )
{
publish_raw_img( img_pose->m_raw_img );
}
if ( g_camera_lidar_queue.m_if_dump_log )
{
g_cost_time_logger.flush();
}
其中里面存在有点云与图像投影的线程以及通过update_pose_for_projection的相机参数更新完成refresh线程的刷新,并最终完成将RGB与雷达点云重合,并完成投影。
5. 参考链接
https://blog.csdn.net/handily_1/article/details/122360134
https://icode.best/i/77363847009772
https://zhuanlan.zhihu.com/p/480275319
https://blog.csdn.net/handily_1/article/details/124230917
评论(0)
您还未登录,请登录后发表或查看评论