VINS初始化
VINS初始化之外参在线标定
前面主要分析了外参标定出来旋转矩阵，接下来接着分析初始化。
if (solver_flag == INITIAL)
{
if (frame_count == WINDOW_SIZE) //WINDOW_SIZE=20
{
bool result = false;
if (ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
{
// cout << "1 initialStructure" << endl;
result = initialStructure();
initial_timestamp = header;
}
if (result)
{
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
f_manager.removeFailures();
cout << "Initialization finish!" << endl;
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();
}
else
frame_count++;
}
else
{
TicToc t_solve;
solveOdometry();
//ROS_DEBUG("solver costs: %fms", t_solve.toc());
if (failureDetection())
{
// ROS_WARN("failure detection!");
failure_occur = 1;
clearState();
setParameter();
// ROS_WARN("system reboot!");
return;
}
TicToc t_margin;
slideWindow();
f_manager.removeFailures();
//ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
// 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];
然后分析一下initialStructure()函数
bool Estimator::initialStructure()
{
TicToc t_sfm;
//check imu observibility
{
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
//frame_count+1:滑动窗口存储的是10帧值，加上最新帧，即11帧
Quaterniond Q[frame_count + 1];
Vector3d T[frame_count + 1];
map<int, Vector3d> sfm_tracked_points;
vector<SFMFeature> sfm_f;
/**********************************************/
struct SFMFeature
{
bool state;
int id;
vector<pair<int,Vector2d>> observation;
double position[3];
double depth;
};
/**********************************************/
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;//特征点id
for (auto &it_per_frame : it_per_id.feature_per_frame)//当前特征点在每一帧图像的坐标
{
imu_j++;
Vector3d pts_j = it_per_frame.point;//当前特征在编号imuj++帧的归一化坐标
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));//tmp_feature.observation存放的是同一个特征
}
sfm_f.push_back(tmp_feature);//所有的特征点
}
Matrix3d relative_R;//???有个疑问，它的初始值是什么，没找到，但是可以cout输出？？？？
Vector3d relative_T;
int l;
if (!relativePose(relative_R, relative_T, l))
{
cout << "Not enough features or parallax; Move device around" << endl;
return false;
}
/*************************************************************/
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);//第i帧和窗口最后一阵的对应特征点的归一化坐标（像素坐标）
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))//判断视差是否大于30而且判断内点数目是否大于12
/**************************************************************/
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;
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;
}
/**************************************************************/
{
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;
}
/*************************************************************/
GlobalSFM sfm;
if (!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
cout << "global SFM failed!" << endl;
marginalization_flag = MARGIN_OLD;
return false;
}
//solve pnp for all frame
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])
{
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])
{
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 << "Not enough points for solve pnp pts_3_vector size " << pts_3_vector.size() << endl;
return false;
}
if (!cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
{
cout << " solve pnp fail!" << endl;
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
{
cout << "misalign visual structure with IMU" << endl;
return false;
}
}
两帧之间三角化，把匹配点加进去
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
内部函数实现具体理论知识可以参考三角化
VINS-MONO解析——初始化(代码部分)
void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
Matrix4d design_matrix = 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);
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);
}
一般来讲，求位姿，2D-2D对极几何只是在第一次使用，也就是没有3D特征点坐标的时候使用，一旦有了特征点，之后都会用3D-2D的方式求位姿。然后会进入PnP求新位姿，然后三角化求新3D坐标的循环中。
pnp:3d->2d
PnP是求解3D到2D点对的运动方法，主要是为了估计相机的位姿。
3D->2D方法不需要使用对极约束，可以在很少的匹配点中获得较好的运动估计，是最重要的姿态估计方法。
解决PnP问题的主要方法：
- P3P：三对点估计位姿
- 直接线性变换（DLT）
- EPnP
- UPnP
- 非线性优化（Bundle Adjustment）
bool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,
vector<SFMFeature> &sfm_f)
{
vector<cv::Point2f> pts_2_vector;
vector<cv::Point3f> pts_3_vector;
for (int j = 0; j < feature_num; j++)
{
if (sfm_f[j].state != true)
continue;
Vector2d point2d;
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
{
if (sfm_f[j].observation[k].first == i)
{
Vector2d img_pts = sfm_f[j].observation[k].second;
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);
pts_3_vector.push_back(pts_3);
break;
}
}
}
if (int(pts_2_vector.size()) < 15)
{
printf("unstable features tracking, please slowly move you device!\n");
if (int(pts_2_vector.size()) < 10)
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(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);
if(!pnp_succ)
{
return false;
}
cv::Rodrigues(rvec, r);
MatrixXd R_pnp;
cv::cv2eigen(r, R_pnp);
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
R_initial = R_pnp;
P_initial = T_pnp;
return true;
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
