香港科技大学的VINS_MONO初试
简介
VINS-Mono 是香港科技大学开源的一个VIO,我简单的测试了，发现效果不错。做个简单的笔记，详细的内容等我毕设搞完再弄。
代码主要分为前端(feature tracker),后端(sliding window, loop closure)，还加了初始化(visual-imu aligment)
Feature tracker
这部分代码在feature_tracker包下面，主要是接收图像topic,使用KLT光流算法跟踪特征点，同时保持每一帧图像有最少的(100-300)个特征点。
根据配置文件中的freq，确定每隔多久的时候，把检测到的特征点打包成/feature_tracker/featuretopic 发出去，
要是没有达到发送的时间，这幅图像的feature就作为下一时刻的
KLT追踪的特征点，就是不是每一副图像都要处理的，那样计算时间大了，而且数据感觉冗余，帧与帧之间图像的差距不会那么明显。
这里的freq配置文件建议至少设置10，为了保证好的前端。
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)
//调用FeatureTracker的readImage
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)));
}
for (unsigned int i = 0;; i++)
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
//更新feature的ID
completed |= trackerData[j].updateID(i);
if (!completed)
break;
}
//发布特征点topic
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
//特征点的id，图像的(u,v)坐标
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;
sensor_msgs::ChannelFloat32 v_of_point;
pub_img.publish(feature_points);
}
if (SHOW_TRACK)
{
//根据特征点被追踪的次数，显示他的颜色，越红表示这个特征点看到的越久，一幅图像要是大部分特征点是蓝色，前端tracker效果很差了，估计要挂了
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
}
}
void FeatureTracker::readImage(const cv::Mat &_img)
{
//直方图均匀化
//if image is too dark or light, trun on equalize to find enough features
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
//根据上一时刻的cur_img,cur_pts,寻找当前时刻的forw_pts,
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
}
if (img_cnt == 0)
{
//根据fundamentalMatrix中的ransac去除一些outlier
rejectWithF();
//跟新特征点track的次数
for (auto &n : track_cnt)
n++;
//为下面的goodFeaturesToTrack保证相邻的特征点之间要相隔30个像素,设置mask image
setMask();
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
//保证每个image有足够的特征点，不够就新提取
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);
}
}
}
Slide Window
主要是对imu的数据进行预积分，vision重投影误差的构造，loop-closure的检测，slide-window的维护 ，marginzation prior的维护，东西比较多。
loop-closure的检测是使用视觉词带的，这里的特征不是feature-tracker的，那样子太少了。是通过订阅IMAGE_TOPIC,传递到闭环检测部分，重新检测的，这个我还没有认真看(做了很多限制，为了搜索的速度，词带不会很大，做了很多限制，从论文上看优化的方程只是加了几个vision重投影的限制，速度不会太慢)。
是只有4个自由度的优化，roll, pitch由于重力对齐的原因是可观测的，就不去优化。
最主要的还是下面这个最小二乘法方程构建，主要的代码我列出来。
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//调用imu的预积分，propagation ,计算对应的雅可比矩阵
//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);
//提供imu计算的当前位置，速度，作为优化的初值
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;
}
}
void Estimator::processImage(const map<int, vector<pair<int, Vector3d>>> &image, const std_msgs::Header &header)
{
//根据视差判断是不是关键帧，
if (f_manager.addFeatureCheckParallax(frame_count, image))
marginalization_flag = MARGIN_OLD;
else
marginalization_flag = MARGIN_SECOND_NEW;
ImageFrame imageframe(image, header.stamp.toSec());
imageframe.pre_integration = tmp_pre_integration;
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
//参数要是设置imu-camera的外参数未知，也可以帮你求解的
if(ESTIMATE_EXTRINSIC == 2)
{
}
//初始化的流程
if (solver_flag == INITIAL)
{
if (frame_count == WINDOW_SIZE)
{
bool result = false;
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
//构造sfm，优化imu偏差，加速度g，尺度的确定
result = initialStructure();
initial_timestamp = header.stamp.toSec();
}
if(result)
{
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();
}
//先凑够window-size的数量的Frame
else
frame_count++;
}
else
{
solveOdometry();
//失败的检测
if (failureDetection())
{
clearState();
setParameter();
return;
}
slideWindow();
f_manager.removeFailures();
// prepare output of VINS
key_poses.clear();
for (int i = 0; i <= WINDOW_SIZE; i++)
key_poses.push_back(Ps[i]);
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
}
void Estimator::slideWindow()
{
//WINDOW_SIZE中的参数的之间调整，同时FeatureManager进行管理feature，有些点要删除掉，有些点的深度要在下一frame表示(start frame已经删除了)
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
//清楚数据，给下一副图像提供空间
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
}
void Estimator::solveOdometry()
{
if (frame_count < WINDOW_SIZE)
return;
if (solver_flag == NON_LINEAR)
{
//三角化点
f_manager.triangulate(Ps, tic, ric);
ROS_DEBUG("triangulation costs %f", t_tri.toc());
optimization();
}
}
void Estimator::optimization()
{
//添加frame的state，(p,v,q,b_a,b_g)，就是ceres要优化的参数
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
//添加camera-imu的外参数
for (int i = 0; i < NUM_OF_CAM; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
}
//为ceres参数赋予初值
vector2double();
//添加margination residual， 先验知识
//他的Evaluate函数看好，固定了线性化的点，First Jacobian Estimate
if (last_marginalization_info)
{
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
//添加imu的residual
for (int i = 0; i < WINDOW_SIZE; 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]);
}
//添加vision的residual
for (auto &it_per_id : f_manager.feature)
{
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
{
continue;
}
Vector3d pts_j = it_per_frame.point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
f_m_cnt++;
}
}
//添加闭环的参数和residual
if(LOOP_CLOSURE)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(front_pose.loop_pose, SIZE_POSE, local_parameterization);
if(front_pose.features_ids[retrive_feature_index] == it_per_id.feature_id)
{
Vector3d pts_j = Vector3d(front_pose.measurements[retrive_feature_index].x, front_pose.measurements[retrive_feature_index].y, 1.0);
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[start], front_pose.loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);
retrive_feature_index++;
loop_factor_cnt++;
}
}
//设置了优化的最长时间，保证实时性
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;
// 求解
ceres::Solve(options, &problem, &summary);
// http://blog.csdn.net/heyijia0327/article/details/53707261#comments
// http://blog.csdn.net/heyijia0327/article/details/52822104
if (marginalization_flag == MARGIN_OLD)
{
//如果当前帧是关键帧的，把oldest的frame所有的信息margination，作为下一时刻的先验知识，参考上面的两个网址，大神的解释很明白
}
else{
//如果当前帧不是关键帧的，把second newest的frame所有的视觉信息丢弃掉，imu信息不丢弃，记住不是做margination，是为了保持矩阵的稀疏性
}
}
后续
imu的参数很重要，还有就是硬件同步，global shutter的摄像头很重要。我要是动作快的话，效果就不行了。但人家的视频感觉效果很不错。
这个还要继续弄硬件和代码原理，代码中最小二乘法优化中的FOCAL_LENGTH感觉要根据自己的摄像头设置，还没有具体看，视觉信息矩阵的设置还没有看。
工程中具体的情况还是要自己解决，
