Vins-Mono 论文 && Coding 一 5(1). 初始化: SFM 初始化

最新推荐文章于 2023-11-01 22:48:37 发布

scofield_ly

最新推荐文章于 2023-11-01 22:48:37 发布

阅读量430

点赞数

分类专栏： # vio 文章标签： slam

本文链接：https://blog.csdn.net/weixin_40072161/article/details/120661764

版权

vio 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

一、初始化流程

二、imu 加速度方差检查

{
        // 1. 计算窗口内加速度均值
        map<double, ImageFrame>::iterator frame_it;
        Vector3d sum_g;
        sum_g.setZero();
        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;  // imu 预积分速度 / 时间 = 加速度
            sum_g += tmp_g;
        }
        Vector3d aver_g;
        aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
        // 2. 计算窗口内加速度方差
        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);
        }
        var = sqrt(var / ((int)all_image_frame.size() - 1));
        // 3. 方差过下，不进行初始化
        if(var < 0.25) {
            ROS_INFO("IMU excitation not enouth!");
            return false;
        }
    }

三、构造视觉特征观测队列

1. 结构体定义

struct SFMFeature {
    bool state;  // construct sfm 是否三角化成功
    int id;
    vector<pair<int,Vector2d>> observation;  // 观测帧id <--> 像素坐标
    double position[3];   // 3 维坐标，三角化初始化，full BA 优化
    double depth;
};

2. 队列更新

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);
}

四、求参考帧 relativePose

1. 参考帧 l 选择

vector<pair<Vector3d, Vector3d>> corres;
corres = f_manager.getCorresponding(i, WINDOW_SIZE);

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 ... ) {
    l = i;
    return true;
}

2. 通过极线约束 solve RT

bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation) {
   // 求解 E 矩阵
   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;
   // 分解 rot,trans, inlier 检查
   int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
   // cv::Mat -> Eigen::Mat
   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);
   } 
   // 求解变换矩阵的 R,t
   Rotation = R.transpose();
   Translation = -R.transpose() * T;
}

(1). 由于这里 cameraMatrix 是单位矩阵，所以求得的 F 矩阵其实就是 E 矩阵

(2). 求出的 Rotation、Translation 对应 $T_{l\_{n-1}}=\bigl(\begin{smallmatrix} Rotation & Translation \\ 0 & 1 \end{smallmatrix}\bigr)$ , $n-1$ 是当前帧（窗口内最新一阵）， $l$ 是 1. 中选取的参考帧 L

五、GlobalSFM Contruct

1. 函数参数

// frame_num: 初始化时窗口内帧的个数
// q: quaterniond 数组名, 窗口内每一帧对应一个 rotation
// t: translation 数组名, 窗口内每一帧对应一个 translation
// l: 参考帧
// relative_R: Rl_n-1
// relative_T: tl_n-1
// sfm_f: 窗口内的所有 feature
// sfm_tracked_points: 被跟踪的 feature
bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
			  const Matrix3d relative_R, const Vector3d relative_T,
			  vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points){
 ......
}

2. 以第 L 帧坐标系作为世界坐标系

q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
T[l].setZero();
q[frame_num - 1] = q[l] * Quaterniond(relative_R);
T[frame_num - 1] = relative_T;

$T_{wl}=\bigl(\begin{smallmatrix} I & 0 \\ 0 & 1 \end{smallmatrix}\bigr)$ , $T_{w\_n-1}=T_{wl}T_{l\_n-1}=T_{l\_n-1}$

3. 局部变量存储 Inverse Pose

这里 c_Rotation, c_Translation, c_Quat, Pose 存储的是 $T_{cw}$ , c_rotation, c_translation 是方便后续 BA

Matrix3d c_Rotation[frame_num];
Vector3d c_Translation[frame_num];
Quaterniond c_Quat[frame_num];
double c_rotation[frame_num][4];
double c_translation[frame_num][3];
Eigen::Matrix<double, 3, 4> Pose[frame_num];

c_Quat[l] = q[l].inverse();
c_Rotation[l] = c_Quat[l].toRotationMatrix();
c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
Pose[l].block<3, 1>(0, 3) = c_Translation[l];

c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];

4. solve frames: l ----- frame_num - 1

//1: trangulate between l ----- frame_num - 1

//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;

//3: triangulate l-----l+1 l+2 ... frame_num -2

for (int i = l; i < frame_num - 1 ; i++) {
   // solve pnp
   if (i > l) {
	Matrix3d R_initial = c_Rotation[i - 1];
    Vector3d P_initial = c_Translation[i - 1];
	if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
		return false;
    c_Rotation[i] = R_initial;
	c_Translation[i] = P_initial;
	c_Quat[i] = c_Rotation[i];
	Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
	Pose[i].block<3, 1>(0, 3) = c_Translation[i];
    }

    // triangulate point based on the solve pnp result
    triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}

for (int i = l + 1; i < frame_num - 1; i++)
     triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);

5. solve frames: l ----- 0

// solve pnp l-1; triangulate l-1 ----- l

// l-2 l-2 ----- l

for (int i = l - 1; i >= 0; i--) {
    //solve pnp
	Matrix3d R_initial = c_Rotation[i + 1];
	Vector3d P_initial = c_Translation[i + 1];
	if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
		return false;
	c_Rotation[i] = R_initial;
	c_Translation[i] = P_initial;
	c_Quat[i] = c_Rotation[i];
	Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
	Pose[i].block<3, 1>(0, 3) = c_Translation[i];
	//triangulate
	triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
}

6. triangulate all other points

for (int j = 0; j < feature_num; j++) {
	if (sfm_f[j].state == true)  // 忽略已经三角化成功的特征点
			continue;
	if ((int)sfm_f[j].observation.size() >= 2) {  // 第一次观测和最后一次观测三角化
		Vector2d point0, point1;
		int frame_0 = sfm_f[j].observation[0].first;
		point0 = sfm_f[j].observation[0].second;
		int frame_1 = sfm_f[j].observation.back().first;
		point1 = sfm_f[j].observation.back().second;
		Vector3d point_3d;
		triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
		sfm_f[j].state = true;
		sfm_f[j].position[0] = point_3d(0);
		sfm_f[j].position[1] = point_3d(1);
		sfm_f[j].position[2] = point_3d(2);
	}		
}

7.full BA, 求解最大似然估计解

ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
// 添加 camera pose state vector
for (int i = 0; i < frame_num; i++) {
	c_translation[i][0] = c_Translation[i].x();
	c_translation[i][1] = c_Translation[i].y();
	c_translation[i][2] = c_Translation[i].z();
	c_rotation[i][0] = c_Quat[i].w();
	c_rotation[i][1] = c_Quat[i].x();
	c_rotation[i][2] = c_Quat[i].y();
	c_rotation[i][3] = c_Quat[i].z();
	problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
	problem.AddParameterBlock(c_translation[i], 3);
	if (i == l) {  // 参考帧 pose 固定
		problem.SetParameterBlockConstant(c_rotation[i]);
	}
	if (i == l || i == frame_num - 1) {  // 最新帧的 translation 固定
		problem.SetParameterBlockConstant(c_translation[i]);
	}
}

// 添加重投影 error term
for (int i = 0; i < feature_num; i++) {
	if (sfm_f[i].state != true)
		continue;
	for (int j = 0; j < int(sfm_f[i].observation.size()); j++) {
		int l = sfm_f[i].observation[j].first;
		ceres::CostFunction* cost_function = ReprojectionError3D::Create(
		sfm_f[i].observation[j].second.x(), sfm_f[i].observation[j].second.y());
        problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], sfm_f[i].position);	 
	}
}

// 求解	
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.minimizer_progress_to_stdout = true;
options.max_solver_time_in_seconds = 0.2;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03) {
	//cout << "vision only BA converge" << endl;
} else {
	//cout << "vision only BA not converge " << endl;
	return false;
}

// 恢复 pose
for (int i = 0; i < frame_num; i++) {
	q[i].w() = c_rotation[i][0]; 
	q[i].x() = c_rotation[i][1]; 
	q[i].y() = c_rotation[i][2]; 
	q[i].z() = c_rotation[i][3]; 
	q[i] = q[i].inverse();
}
for (int i = 0; i < frame_num; i++) {
    T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
}
for (int i = 0; i < (int)sfm_f.size(); i++) {
	if(sfm_f[i].state)
		sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
}

8. 公共函数

(1). triangulateTwoFrames && triangulatePoint

// frame0: 三角化第一帧索引
// Pose0: 第一帧 pose
// frame1: 三角化第二帧索引
// Pose1: 第二帧 pose
// sfm_f: sfm 管理的 features
void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0, int frame1, Eigen::Matrix<double, 3, 4> &Pose1, vector<SFMFeature> &sfm_f)
{
	assert(frame0 != frame1);
	for (int j = 0; j < feature_num; j++)
	{
		if (sfm_f[j].state == true)  // 已经三角化
			continue;
		bool has_0 = false, has_1 = false;
		Vector2d point0;
		Vector2d point1;
		for (int k = 0; k < (int)sfm_f[j].observation.size(); k++) {
			if (sfm_f[j].observation[k].first == frame0) {
				point0 = sfm_f[j].observation[k].second;
				has_0 = true;
			}
			if (sfm_f[j].observation[k].first == frame1) {
				point1 = sfm_f[j].observation[k].second;
				has_1 = true;
			}
		}
		if (has_0 && has_1) { // 该 feature 被 frame0 和 frame1 同时观测到
			Vector3d point_3d;
			triangulatePoint(Pose0, Pose1, point0, point1, point_3d);
			sfm_f[j].state = true;
			sfm_f[j].position[0] = point_3d(0);  // 更新三角化结果
			sfm_f[j].position[1] = point_3d(1);
			sfm_f[j].position[2] = point_3d(2);
		}							  
	}
}

// Pose0: 第1帧 pose
// Pose1: 第2帧 pose
// point1: 第一帧 uv 坐标
// point2: 第二帧 uv 坐标
// point_3d: 三角化得到的三维坐标
/**
  原理: DLT 算法三角化
  P1X=x1; P2X=x2
  P1X cross_product x1 = 0
  得到 (x(p3T)-P1T)X=0
      (y(p3T)-p2T)X=0
  AX=0, SVD 分解可以求得最小二乘解
*/
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=design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();  // SVD 分解
	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);  // 齐次坐标化为3维
}

(2). solveFrameByPnP

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) {  // 仅仅挑选 i, j 帧的观测点
		    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;
		}
	}
}

......
pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);

六、solve pnp for all frame

1. 实现

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 from global sfm construct
    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);  // 从 map-points 中选点
            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);
            }
        }
    }
    // pnp 求解
    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;
}