为什么要初始化

非线性VINS估计器的性能对于初始的速度,尺度,重力向量,空间点3D位置,以及外参等非常敏感。在很多场合中,能做到相机和IMU即插即用,线上自动校准与初始化,将会给用户带来极大的方便性。VINS里面分四步进行,第一个就是上次讲的旋转外参校准,第二个就是找到某帧作为系统初始化原点,计算3D地图点,第三就是将相机坐标系转到IMU坐标系中,第四就是相机与IMU对齐,包括IMU零偏初始化,速度,重力向量,尺度初始化

初始化系统原点与转换到IMU坐标系

bool Estimator::initialStructure()
{
TicToc t_sfm;
//check imu observibility, 通过协方差检测IMU的可观测性
{
map<double, ImageFrame>::iterator frame_it;
Vector3d sum_g;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
sum_g += tmp_g;
}
Vector3d aver_g;
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
double var = 0;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
//cout << "frame g " << tmp_g.transpose() << endl;
}
var = sqrt(var / ((int)all_image_frame.size() - 1));
//ROS_WARN("IMU variation %f!", var);
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
}
}
// global sfm 准备
Quaterniond Q[frame_count + 1];
Vector3d T[frame_count + 1];
map<int, Vector3d> sfm_tracked_points;
vector<SFMFeature> sfm_f;
for (auto &it_per_id : f_manager.feature)
{
int imu_j = it_per_id.start_frame - 1;
SFMFeature tmp_feature;
tmp_feature.state = false;
tmp_feature.id = it_per_id.feature_id;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Vector3d pts_j = it_per_frame.point;
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
}
sfm_f.push_back(tmp_feature);
}
//在窗口内选择跟最后一帧视差最大的帧,利用五点法计算相对旋转和平移量
Matrix3d relative_R;
Vector3d relative_T;
int l;
if (!relativePose(relative_R, relative_T, l))
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
} // 为了更方便理解,把relativePose 函数取了出来
| ----------------------------------------------------------------------------------------------------------
| bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
| {
| // find previous frame which contians enough correspondance and parallex with newest frame,循环找出和最新帧视差最大的历史帧
| for (int i = 0; i < WINDOW_SIZE; i++)
| {
| vector<pair<Vector3d, Vector3d>> corres;
| corres = f_manager.getCorresponding(i, WINDOW_SIZE);
| if (corres.size() > 20)
| {
| double sum_parallax = 0;
| double average_parallax;
| for (int j = 0; j < int(corres.size()); j++)
| {
| Vector2d pts_0(corres[j].first(0), corres[j].first(1));
| Vector2d pts_1(corres[j].second(0), corres[j].second(1));
| double parallax = (pts_0 - pts_1).norm();
| sum_parallax = sum_parallax + parallax;
| }
| average_parallax = 1.0 * sum_parallax / int(corres.size());
| // 找到后就可以利用五点法计算,如果计算失败就继续循环
| if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
| {
| l = i;
| ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
| return true;
| }
| }
| }
| return false;
| }
| // 核心的五点法,也是求本质矩阵
| bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
| {
| if (corres.size() >= 15)
| {
| vector<cv::Point2f> ll, rr;
| for (int i = 0; i < int(corres.size()); i++)
| {
| ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
| rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
| }
| cv::Mat mask;
| cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
| cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
| cv::Mat rot, trans;
| // recoverPose内用了一些复杂的条件判断如何选取R和T,但是其用了单位矩阵作为cameraMatrix,feature_tracker_node在处理图像的是已经把图像坐标转为归一化的相机坐标了,并且进行了去畸变,所以之后的重投影部分只需要外参,不再需要内参了.
| int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
| //cout << "inlier_cnt " << inlier_cnt << endl;
|
| Eigen::Matrix3d R;
| Eigen::Vector3d T;
| for (int i = 0; i < 3; i++)
| {
| T(i) = trans.at<double>(i, 0);
| for (int j = 0; j < 3; j++)
| R(i, j) = rot.at<double>(i, j);
| }
|
| Rotation = R.transpose();
| Translation = -R.transpose() * T;
| if(inlier_cnt > 12)
| return true;
| else
| return false;
| }
| return false;
| }
| ---------------------------------------------------------------------------------------------------------- GlobalSFM sfm;
/* sfm.construct 函数本身就很巨大,里面很多步骤,代码就不贴了,大致如下:
1. 三角化l 帧与frame_num - 1帧,得到一些地图点。
2. 通过这些地图点,利用pnp的方法计算l+1, l+2, l+3, …… frame_num-2 相对于l的位姿。而且每计算一帧,就会与frame_num - 1帧进行三角化,得出更多地图点
3. 三角化l+1, l+2 …… frame_num-2帧与l帧
4. 对l-1, l-2, l-3 等帧与sfm_f的特征点队列进行pnp求解,得出相对于l的位姿,并三角化其与l帧。
5. 三角化剩余的点
6. ceres全局BA优化,最小化3d投影误差
*/
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
} //solve pnp for all frame,因为有continue存在,怎么会solve all,感觉只是不满足时间戳相等的才pnp求解
map<double, ImageFrame>::iterator frame_it;
map<int, Vector3d>::iterator it;
frame_it = all_image_frame.begin( );
for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
{
// provide initial guess
cv::Mat r, rvec, t, D, tmp_r;
if((frame_it->first) == Headers[i].stamp.toSec())
{
frame_it->second.is_key_frame = true;
frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
frame_it->second.T = T[i];
i++;
continue;
}
if((frame_it->first) > Headers[i].stamp.toSec())
{
i++;
}
Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
Vector3d P_inital = - R_inital * T[i];
cv::eigen2cv(R_inital, tmp_r);
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_inital, t); frame_it->second.is_key_frame = false;
vector<cv::Point3f> pts_3_vector;
vector<cv::Point2f> pts_2_vector;
for (auto &id_pts : frame_it->second.points)
{
int feature_id = id_pts.first;
for (auto &i_p : id_pts.second)
{
it = sfm_tracked_points.find(feature_id);
if(it != sfm_tracked_points.end())
{
Vector3d world_pts = it->second;
cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
pts_3_vector.push_back(pts_3);
Vector2d img_pts = i_p.second.head<2>();
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
}
}
}
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
if(pts_3_vector.size() < 6)
{
cout << "pts_3_vector size " << pts_3_vector.size() << endl;
ROS_DEBUG("Not enough points for solve pnp !");
return false;
}
if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
{
ROS_DEBUG("solve pnp fail!");
return false;
}
cv::Rodrigues(rvec, r);
MatrixXd R_pnp,tmp_R_pnp;
cv::cv2eigen(r, tmp_R_pnp);
R_pnp = tmp_R_pnp.transpose();
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
T_pnp = R_pnp * (-T_pnp);
frame_it->second.R = R_pnp * RIC[0].transpose();
frame_it->second.T = T_pnp;
}
if (visualInitialAlign())
return true;
else
{
ROS_INFO("misalign visual structure with IMU");
return false;
} }

相机与IMU对齐

bool Estimator::visualInitialAlign()
{
TicToc t_g;
VectorXd x;
/*里面包含
1. solveGyroscopeBias()零偏初始化: 在滑动窗口中,每两帧之间的相对旋转与IMU预积分产生的旋转进行最小二乘法优化,用ldlt求解
2. LinearAlignment() 尺度初始化:由于在视觉初始化SFM的过程中,将其中位姿变化较大的两帧中使用E矩阵求解出来旋转和位移,后续的PnP和三角化都是在这个尺度下完成的。所以当前的尺度与IMU测量出来的真实世界尺度肯定不是一致的,所以需要这里进行对齐。这里对齐的方法主要是通过在滑动窗口中每两帧之间的位置和速度与IMU预积分出来的位置和速度组成一个最小二乘法的形式,然后求解出来
3. RefineGravity()重力向量优化:进一步细化重力加速度,提高估计值的精度,形式与LinearAlignment()是一致的,只是将g改为g⋅ĝ +w1b1+w2b2
2和3的公式相对难懂,请参考下面图片
*/
bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
if(!result)
{
ROS_DEBUG("solve g failed!");
return false;
} // change state
for (int i = 0; i <= frame_count; i++)
{
Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
Ps[i] = Pi;
Rs[i] = Ri;
all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
} VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < dep.size(); i++)
dep[i] = -1;
f_manager.clearDepth(dep); //triangulat on cam pose , no tic三角化特征点,
Vector3d TIC_TMP[NUM_OF_CAM];
for(int i = 0; i < NUM_OF_CAM; i++)
TIC_TMP[i].setZero();
ric[0] = RIC[0];
f_manager.setRic(ric);
f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
//更新相机速度,位置和旋转量(通过精确求解的尺度,重力向量)
double s = (x.tail<1>())(0);
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
for (int i = frame_count; i >= 0; i--)
Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
int kv = -1;
map<double, ImageFrame>::iterator frame_i;
for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
{
if(frame_i->second.is_key_frame)
{
kv++;
Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
}
}
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
it_per_id.estimated_depth *= s;
} Matrix3d R0 = Utility::g2R(g);
double yaw = Utility::R2ypr(R0 * Rs[0]).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
g = R0 * g;
//Matrix3d rot_diff = R0 * Rs[0].transpose();
Matrix3d rot_diff = R0;
for (int i = 0; i <= frame_count; i++)
{
Ps[i] = rot_diff * Ps[i];
Rs[i] = rot_diff * Rs[i];
Vs[i] = rot_diff * Vs[i];
}
ROS_DEBUG_STREAM("g0 " << g.transpose());
ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose()); return true;
}

VINS 估计器之结构初始化的更多相关文章

  1. VINS 估计器之优化与边缘化

    VINS的优化除了添加了投影残差,回环检测残差,还有IMU的残差,边缘化产生的先验信息残差等.有些比较难理解,可参考此博客和知乎回答. void Estimator::optimization() { ...

  2. VINS 估计器之检查视差

    为什么检查视差 VINS里为了控制优化计算量,在实时情况下,只对当前帧之前某一部分帧进行优化,而不是全部历史帧.局部优化帧的数量就是窗口大小.为了维持窗口大小,需要去除旧的帧添加新的帧,也就是边缘化 ...

  3. C语言标记化结构初始化语法

    C语言标记化结构初始化语法 (designated initializer),而且还是一个ISO标准. #include <stdio.h> #include <stdlib.h&g ...

  4. 标记化结构初始化语法 在结构体成员前加上小数点 如 “.open .write .close ”C99编译器 .

    今天在看串口驱动(四)的时候 有这样一个结构体初始化 我很不理解 如下: static struct s3c24xx_uart_port s3c24xx_serial_ports[NR_PORTS] ...

  5. VINS 估计器之外参初始化

    为何初始化外参 当外参完全不知道的时候,VINS也可以在线对其进行估计(rotation),先在processImage内进行初步估计,然后在后续优化时,会在optimize函数中再次优化. 如何初始 ...

  6. VINS(四)初始化与相机IMU外参标定

    和单目纯视觉的初始化只需要获取R,t和feature的深度不同,VIO的初始化话通常需要标定出所有的关键参数,包括速度,重力方向,feature深度,以及相机IMU外参$R_{c}^{b}$和$p_{ ...

  7. VINS(八)初始化

    首先通过imu预积分陀螺仪和视觉特征匹配分解Fundamental矩阵获取rotationMatrix之间的约束关系,联立方程组可以求得外参旋转矩阵: 接下来会检测当前frame_count是否达到W ...

  8. php-5.6.26源代码 - hash存储结构 - 初始化

    初始化 有指定析构函数,在销毁hash的时候会调用,如:“类似extension=test.so扩展”也是存放在HashTable中的,“类似extension=test.so扩展”的module_s ...

  9. 标准C的标记化结构初始化语法

    1 struct file_operations { 2         struct module *owner; 3         loff_t (*llseek) (struct file * ...

随机推荐

  1. android中与SQLite数据库相关的类

    为什么要在应用程序中使用数据库?数据库最主要的用途就是作为数据的存储容器,另外,由于可以很方便的将应用程序中的数据结构(比如C语言中的结构体)转化成数据库的表,这样我们就可以通过操作数据库来替代写一堆 ...

  2. 关于hadoop集群下Datanode和Namenode无法访问的解决方案

    HDFS架构 HDFS也是按照Master和Slave的结构,分namenode,secondarynamenode,datanode这几个角色. Namenode:是maseter节点,是大领导.管 ...

  3. C语言指针作业

    一.PTA实验作业 题目1:6-5 判断回文字符串 1. 本题PTA提交列表 2. 设计思路 3.代码截图 4.本题调试过程碰到问题及PTA提交列表情况说明. 第一次做的时候我j直接等于count,其 ...

  4. 201621123050 《Java程序设计》第3周学习总结

    1. 本周学习总结 初学面向对象,会学习到很多碎片化的概念与知识.尝试学会使用思维导图将这些碎片化的概念.知识点组织起来.请使用工具画出本周学习到的知识点及知识点之间的联系.步骤如下: 1.1 写出你 ...

  5. Flask jinja2 全局函数,宏

    内置全局函数 dict()函数,方便生成字典型变量 {% set user = dict(name='Mike',age=15) %} <p>{{ user | tojson | safe ...

  6. Mego(1) - NET中主流ORM框架性能对比

    从刚刚开始接触ORM到现在已有超过八年时间,用过了不少ORM框架也了解了不少ORM框架,看过N种关于ORM框架的相关资料与评论,各种言论让人很难选择.在ORM的众多问题中最突出的问题是关于性能方面的问 ...

  7. Web Api 过滤器之 AuthorizationFilter 验证过滤器

    该过滤器是最先执行的过滤器,即使把它放在最后 API [MyActionFilter] [MyExceptionFilter] [MyAuthorize] public void Get() { Tr ...

  8. 20165230 2017-2018-2 《Java程序设计》第4周学习总结

    20165230 2017-2018-2 <Java程序设计>第4周学习总结 教材学习内容总结 子类与继承 通过class 子类名 extends 父类名定义子类.子类只能继承一个父类,关 ...

  9. 新概念英语(1-63)Thank you, doctor.

    新概念英语(1-63)Thank you, doctor. Who else is in bed today? why? A:How's Jimmy today? B:Better. Thank yo ...

  10. C# 后台构造json数据

    前后台传值一般情况下,都会用到json类型的数据,比较常见,但是每次用到的时候去网上找比较麻烦,所以自己记录一下,下次直接用. 构造的json串格式,如下: [{","name&q ...