香港科技大学的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/feature
topic 发出去,
要是没有达到发送的时间,这幅图像的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
感觉要根据自己的摄像头设置,还没有具体看,视觉信息矩阵的设置还没有看。
工程中具体的情况还是要自己解决,
香港科技大学的VINS_MONO初试的更多相关文章
- 学习《Hardware-Efficient Bilateral Filtering for Stereo Matching》一文笔记。
个人收藏了很多香港大学.香港科技大学以及香港中文大学里专门搞图像研究一些博士的个人网站,一般会不定期的浏览他们的作品,最近在看杨庆雄的网点时,发现他又写了一篇双边滤波的文章,并且配有源代码,于是下载下 ...
- [SLAM] Studying Guidance
Books from Zhihu: 幽默一把 看完Gonzalez:嗯,好像很好玩的样子,我也来搞一搞.看完Price:什么鬼,怎么这么多公式,公式看不懂肿么破.看完Szeliski:原来用一千页的书 ...
- China Brain Project: Basic Neuroscience, Brain Diseases, and Brain-Inspired Computing
日前,中国科学院神经科学研究所.中国科学院脑科学与智能技术卓越创新中心.香港科技大学生命科学部和分子神经科学国家重点实验室.中国科技大学自动化研究所在 Cell 上联合发表了一篇概述论文<Chi ...
- 269. Alien Dictionary
题目: There is a new alien language which uses the latin alphabet. However, the order among letters ar ...
- MapReduce原理及其主要实现平台分析
原文:http://www.infotech.ac.cn/article/2012/1003-3513-28-2-60.html MapReduce原理及其主要实现平台分析 亢丽芸, 王效岳, 白如江 ...
- 2016年QS亚洲大学排行榜
2016年QS亚洲大学排行榜 人民网北京6月14日电 (记者 郝孟佳)今天,全球高等教育分析机构QS全球教育集团发布了2016年QS亚洲大学排名.清华大学在亚洲20强大学中进步最大,比去年提升6名,上 ...
- 数据可视化的优秀入门书籍有哪些,D3.js 学习资源汇总
习·D3.js 学习资源汇总 除了D3.js自身以外,许多可视化工具包都是基于D3开发的,所以对D3的学习就显得很重要了,当然如果已经有了Javascript的经验,学起来也会不费力些. Github ...
- 计算机专业-世界大学学术排名,QS排名,U.S.NEWS排名
2015年美国大学计算机专业排名 计算机专业介绍:计算机涉及的领域非常广泛,其分支学科也是非常多.所以在美国将主要的专业方向分为人工智能,程序应用,计算机系统(Systems)以及计算机理论(theo ...
- shufe前辈名师
前辈名师 姓名 现职/原职 郭秉文 中国现代大学之父.国立东南大学校长.哥伦比亚大学教育学博士,该校第一任校长.为了纪念郭秉文先生,勉励优秀学子,郭夏瑜女士在上海财经大学等校设立了“郭秉文奖学金” 马 ...
随机推荐
- Spring配置数据源的几种方法
一:数据源的配置1.通过JNDI配置数据源 1.在tomcat context.xml中配置数据源 <Resource name="jdbc/ds" au ...
- 因为文件组 'PRIMARY' 已满 解决办法
简介:文件组 'PRIMARY' 已满 一般虚拟主机提供商是通过限制数据库文件的大小来实现提供定制的数据库空间的.当你把从虚拟数据库空间备份下来的文件恢复到自己的服务器上时,这个限制还是存在的.找到数 ...
- .NET Framework 4.7 安装
我们打开.NET Framework下载界面: https://www.microsoft.com/net/download/framework 这时你会发现,我们能下载的.NET Framework ...
- C++小技巧之CONTAINING_RECORD
CONTAINING_RECORD Containing record是一个在C++编程中用处很大的一种技巧,它的功能为已知结构体或类的某一成员.对象中该成员的地址以及这一结构体名或类名,从而得到该对 ...
- openssl命令行-证书认证
命令1: openssl genrsa -out root.key 1024 产生一个root.key的私钥 命令2: openssl req -key root.key -new -out ro ...
- 物理提取大绝招”Advanced ADB”???
近来手机取证有个极为重大的突破,是由手机取证大厂Cellebrite所率先发表的"Advanced ADB" 物理提取方法,此功能已纳入其取证设备产品UFED 6.1之中. 这个所 ...
- bootstrap快速入门笔记(三)响应式,行,列,偏移量,排序
一,响应式列重置 .clearfix <div class="row"> <div class="col-xs-6 col-sm-3"> ...
- html常用的知识点以及混合框架
html中: <hr/> 在页面中创建水平线 例如: <p> p标签是定义段落 > alt 作为可预备可替换信息,在无法加载图片时显示文字信息 定义htm ...
- [KISSY5系列]KISSY5安装使用(二)
上篇文章,我们遗留的一个问题就是,请求数量有点多,虽然现在的浏览器的请求资源都是并发的.但是我们还是尽量减少请求量. 但是KISSY里面这个问题我们怎么解决呢,其实还是从data-config=&qu ...
- shell入门笔记1:执行方式、运行方式、变量、替换
说明: 本文是关于http://c.biancheng.net/cpp/shell/的相关笔记 shell的两种执行方式 交互式(interactive) 解释执行用户的命令,用户输入一条命令,She ...