【VINS-Fusion入门之二】基于优化的多传感器融合

2023-05-16

文章目录

  • 简介
    • 特征
    • 参考论文
  • 解读
    • 系统框图
    • 相机模型:
    • 配置文件
    • 全局优化
    • 闭环优化
    • 状态估计

简介

VINS-Fusion is an optimization-based multi-sensor state estimator, which achieves accurate self-localization for autonomous applications (drones, cars, and AR/VR). VINS-Fusion is an extension of VINS-Mono, which supports multiple visual-inertial sensor types (mono camera + IMU, stereo cameras + IMU, even stereo cameras only). We also show a toy example of fusing VINS with GPS.

特征

// 多相机
multiple sensors support (stereo cameras / mono camera+IMU / stereo cameras+IMU)
// 在线空间标定
online spatial calibration (transformation between camera and IMU)
// 在线时域标定
online temporal calibration (time offset between camera and IMU)
// 闭环
visual loop closure

参考论文

// 基于优化的多传感器的局部里程计估计
A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors, Tong Qin, Jie Pan, Shaozu Cao, Shaojie Shen, aiXiv 
// 基于优化的多传感器的全局位姿估计
A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors, Tong Qin, Shaozu Cao, Jie Pan, Shaojie Shen, aiXiv
// 单目视觉惯性系统的在线实时标定
Online Temporal Calibration for Monocular Visual-Inertial Systems, Tong Qin, Shaojie Shen, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS, 2018)
// 一种鲁棒多用途的单目视觉惯性状态估计量
VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Shaojie Shen, IEEE Transactions on Robotics

解读

系统框图

该图指示全并行单目VINS设计的方法,包括初始化过程,重定位模块,位姿图优化模块。
Block Diagram
The system starts with measurement preprocessing (see Section IV), in which features are extracted and tracked, and IMU measurements between two consecutive frames are preintegrated. The initialization procedure (see Section V) provides all necessary values, including, pose, velocity, gravity vector, gyroscope bias, and three-dimensional (3-D) feature location, for bootstrapping the subsequent nonlinear optimization-based VIO. The VIO (see Section VI) with relocalization (see Section VII) modules tightly fuses preintegrated IMU measurements, feature observations. Finally, the pose graph optimization module (see Section VIII) takes in geometrically verified relocalization results, and perform global optimization to eliminate the drift. It also achieves the pose graph reuse. The VIO and pose graph optimization modules run concurrently in separated threads.

相机模型:

  1. 标定

计算重投影误差:

double
Camera::reprojectionError(const std::vector< std::vector<cv::Point3f> >& objectPoints,
                          const std::vector< std::vector<cv::Point2f> >& imagePoints,
                          const std::vector<cv::Mat>& rvecs,
                          const std::vector<cv::Mat>& tvecs,
                          cv::OutputArray _perViewErrors) const
{
    int imageCount = objectPoints.size();
    size_t pointsSoFar = 0;
    double totalErr = 0.0;

    bool computePerViewErrors = _perViewErrors.needed();
    cv::Mat perViewErrors;
    if (computePerViewErrors)
    {
        _perViewErrors.create(imageCount, 1, CV_64F);
        perViewErrors = _perViewErrors.getMat();
    }

    for (int i = 0; i < imageCount; ++i)
    {
        size_t pointCount = imagePoints.at(i).size();

        pointsSoFar += pointCount;

        std::vector<cv::Point2f> estImagePoints;
        projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i),
                      estImagePoints);

        double err = 0.0;
        for (size_t j = 0; j < imagePoints.at(i).size(); ++j)
        {
            err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));
        }

        if (computePerViewErrors)
        {
            perViewErrors.at<double>(i) = err / pointCount;
        }

        totalErr += err;
    }

    return totalErr / pointsSoFar;
}

计算畸变校正映射表:

cv::Mat
PinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
                                       float fx, float fy,
                                       cv::Size imageSize,
                                       float cx, float cy,
                                       cv::Mat rmat) const
{
    if (imageSize == cv::Size(0, 0))
    {
        imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());
    }

    cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
    cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);

    Eigen::Matrix3f R, R_inv;
    cv::cv2eigen(rmat, R);
    R_inv = R.inverse();

    // assume no skew
    Eigen::Matrix3f K_rect;

    if (cx == -1.0f || cy == -1.0f)
    {
        K_rect << fx, 0, imageSize.width / 2,
                  0, fy, imageSize.height / 2,
                  0, 0, 1;
    }
    else
    {
        K_rect << fx, 0, cx,
                  0, fy, cy,
                  0, 0, 1;
    }

    if (fx == -1.0f || fy == -1.0f)
    {
        K_rect(0,0) = mParameters.fx();
        K_rect(1,1) = mParameters.fy();
    }

    Eigen::Matrix3f K_rect_inv = K_rect.inverse();

    for (int v = 0; v < imageSize.height; ++v)
    {
        for (int u = 0; u < imageSize.width; ++u)
        {
            Eigen::Vector3f xo;
            xo << u, v, 1;

            Eigen::Vector3f uo = R_inv * K_rect_inv * xo;

            Eigen::Vector2d p;
            spaceToPlane(uo.cast<double>(), p);

            mapX.at<float>(v,u) = p(0);
            mapY.at<float>(v,u) = p(1);
        }
    }

    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);

    cv::Mat K_rect_cv;
    cv::eigen2cv(K_rect, K_rect_cv);
    return K_rect_cv;
}

计算雅克比矩阵:

bool
EigenQuaternionParameterization::ComputeJacobian(const double* x,
                                                 double* jacobian) const
{
    jacobian[0] =  x[3]; jacobian[1]  =  x[2]; jacobian[2]  = -x[1];  // NOLINT
    jacobian[3] = -x[2]; jacobian[4]  =  x[3]; jacobian[5]  =  x[0];  // NOLINT
    jacobian[6] =  x[1]; jacobian[7] = -x[0]; jacobian[8] =  x[3];  // NOLINT
    jacobian[9] = -x[0]; jacobian[10]  = -x[1]; jacobian[11]  = -x[2];  // NOLINT
    return true;
}
  1. Model
    作者枚举了以下几种相机模型:
enum ModelType
{
	    KANNALA_BRANDT,
	    MEI,
	    PINHOLE,
	    PINHOLE_FULL,
	    SCARAMUZZA
};

单目畸变:

/**
 * \brief Apply distortion to input point (from the normalised plane)
 *        and calculate Jacobian
 *
 * \param p_u undistorted coordinates of point on the normalised plane
 * \return to obtain the distorted point: p_d = p_u + d_u
 */
void
PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,
                          Eigen::Matrix2d& J) const
{
    double k1 = mParameters.k1();
    double k2 = mParameters.k2();
    double p1 = mParameters.p1();
    double p2 = mParameters.p2();

    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;

    mx2_u = p_u(0) * p_u(0);
    my2_u = p_u(1) * p_u(1);
    mxy_u = p_u(0) * p_u(1);
    rho2_u = mx2_u + my2_u;
    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
           p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);

    double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0);
    double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1);
    double dxdmy = dydmx;
    double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0);

    J << dxdmx, dxdmy,
         dydmx, dydmy;
}

配置文件

主要包含各种数据集的配置信息,以yaml格式保存:

A3_ptgrey/
euroc/
extrinsic_parameter_example.pdf
fisheye_mask_752x480.jpg
fisheye_mask.jpg*
kitti_odom/
kitti_raw/
mynteye/
simulation/
vi_car/
vins_rviz_config.rviz

我们可以看到支持国内小觅双目摄像头。

全局优化

  1. 因子图:(略)
  2. 全局优化:
    基于ceres的优化库,构建cost_function,利用SPARSE_NORMAL_CHOLESKY迭代法,得到最优解。主要包括优化路径和姿态。
void GlobalOptimization::optimize()
{
    while(true)
    {
        if(newGPS)
        {
            newGPS = false;
            printf("global optimization\n");
            TicToc globalOptimizationTime;

            ceres::Problem problem;
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            //options.minimizer_progress_to_stdout = true;
            //options.max_solver_time_in_seconds = SOLVER_TIME * 3;
            options.max_num_iterations = 5;
            ceres::Solver::Summary summary;
            ceres::LossFunction *loss_function;
            loss_function = new ceres::HuberLoss(1.0);
            ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();

            //add param
            mPoseMap.lock();
            int length = localPoseMap.size();
            // w^t_i   w^q_i
            double t_array[length][3];
            double q_array[length][4];
            map<double, vector<double>>::iterator iter;
            iter = globalPoseMap.begin();
            for (int i = 0; i < length; i++, iter++)
            {
                t_array[i][0] = iter->second[0];
                t_array[i][1] = iter->second[1];
                t_array[i][2] = iter->second[2];
                q_array[i][0] = iter->second[3];
                q_array[i][1] = iter->second[4];
                q_array[i][2] = iter->second[5];
                q_array[i][3] = iter->second[6];
                problem.AddParameterBlock(q_array[i], 4, local_parameterization);
                problem.AddParameterBlock(t_array[i], 3);
            }

            map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
            int i = 0;
            for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
            {
                //vio factor
                iterVIONext = iterVIO;
                iterVIONext++;
                if(iterVIONext != localPoseMap.end())
                {
                    Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
                    Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
                    wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4], 
                                                               iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
                    wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
                    wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4], 
                                                               iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
                    wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
                    Eigen::Matrix4d iTj = wTi.inverse() * wTj;
                    Eigen::Quaterniond iQj;
                    iQj = iTj.block<3, 3>(0, 0);
                    Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);

                    ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
                                                                                iQj.w(), iQj.x(), iQj.y(), iQj.z(),
                                                                                0.1, 0.01);
                    problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
                }
                //gps factor
                double t = iterVIO->first;
                iterGPS = GPSPositionMap.find(t);
                if (iterGPS != GPSPositionMap.end())
                {
                    ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1], 
                                                                       iterGPS->second[2], iterGPS->second[3]);
                    //printf("inverse weight %f \n", iterGPS->second[3]);
                    problem.AddResidualBlock(gps_function, loss_function, t_array[i]);

                }

            }
            //mPoseMap.unlock();
            ceres::Solve(options, &problem, &summary);
            //std::cout << summary.BriefReport() << "\n";

            // update global pose
            //mPoseMap.lock();
            iter = globalPoseMap.begin();
            for (int i = 0; i < length; i++, iter++)
            {
            	vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
            							  q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
            	iter->second = globalPose;
            	if(i == length - 1)
            	{
            	    Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity(); 
            	    Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
            	    double t = iter->first;
            	    WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4], 
            	                                                       localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();
            	    WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);
            	    WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4], 
            	                                                        globalPose[5], globalPose[6]).toRotationMatrix();
            	    WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);
            	    WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
            	}
            }
            updateGlobalPath();
            //printf("global time %f \n", globalOptimizationTime.toc());
            mPoseMap.unlock();
        }
        std::chrono::milliseconds dura(2000);
        std::this_thread::sleep_for(dura);
    }
	return;
}

闭环优化

  1. 图优化
    添加关键帧,判断是否检测到回环,如果是,就对关键帧进行特征点提取,利用最优化得到路径优化。
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{
    //shift to base frame
    Vector3d vio_P_cur;
    Matrix3d vio_R_cur;
    if (sequence_cnt != cur_kf->sequence)
    {
        sequence_cnt++;
        sequence_loop.push_back(0);
        w_t_vio = Eigen::Vector3d(0, 0, 0);
        w_r_vio = Eigen::Matrix3d::Identity();
        m_drift.lock();
        t_drift = Eigen::Vector3d(0, 0, 0);
        r_drift = Eigen::Matrix3d::Identity();
        m_drift.unlock();
    }
    
    cur_kf->getVioPose(vio_P_cur, vio_R_cur);
    vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
    vio_R_cur = w_r_vio *  vio_R_cur;
    cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
    cur_kf->index = global_index;
    global_index++;
	int loop_index = -1;
    if (flag_detect_loop)
    {
        TicToc tmp_t;
        loop_index = detectLoop(cur_kf, cur_kf->index);
    }
    else
    {
        addKeyFrameIntoVoc(cur_kf);
    }
	if (loop_index != -1)
	{
        //printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
        KeyFrame* old_kf = getKeyFrame(loop_index);

        if (cur_kf->findConnection(old_kf))
        {
            if (earliest_loop_index > loop_index || earliest_loop_index == -1)
                earliest_loop_index = loop_index;

            Vector3d w_P_old, w_P_cur, vio_P_cur;
            Matrix3d w_R_old, w_R_cur, vio_R_cur;
            old_kf->getVioPose(w_P_old, w_R_old);
            cur_kf->getVioPose(vio_P_cur, vio_R_cur);

            Vector3d relative_t;
            Quaterniond relative_q;
            relative_t = cur_kf->getLoopRelativeT();
            relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
            w_P_cur = w_R_old * relative_t + w_P_old;
            w_R_cur = w_R_old * relative_q;
            double shift_yaw;
            Matrix3d shift_r;
            Vector3d shift_t; 
            shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
            shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
            shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur; 
            // shift vio pose of whole sequence to the world frame
            if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
            {  
                w_r_vio = shift_r;
                w_t_vio = shift_t;
                vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                vio_R_cur = w_r_vio *  vio_R_cur;
                cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
                list<KeyFrame*>::iterator it = keyframelist.begin();
                for (; it != keyframelist.end(); it++)   
                {
                    if((*it)->sequence == cur_kf->sequence)
                    {
                        Vector3d vio_P_cur;
                        Matrix3d vio_R_cur;
                        (*it)->getVioPose(vio_P_cur, vio_R_cur);
                        vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                        vio_R_cur = w_r_vio *  vio_R_cur;
                        (*it)->updateVioPose(vio_P_cur, vio_R_cur);
                    }
                }
                sequence_loop[cur_kf->sequence] = 1;
            }
            m_optimize_buf.lock();
            optimize_buf.push(cur_kf->index);
            m_optimize_buf.unlock();
        }
	}
	m_keyframelist.lock();
    Vector3d P;
    Matrix3d R;
    cur_kf->getVioPose(P, R);
    P = r_drift * P + t_drift;
    R = r_drift * R;
    cur_kf->updatePose(P, R);
    Quaterniond Q{R};
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
    pose_stamped.header.frame_id = "world";
    pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
    pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
    pose_stamped.pose.position.z = P.z();
    pose_stamped.pose.orientation.x = Q.x();
    pose_stamped.pose.orientation.y = Q.y();
    pose_stamped.pose.orientation.z = Q.z();
    pose_stamped.pose.orientation.w = Q.w();
    path[sequence_cnt].poses.push_back(pose_stamped);
    path[sequence_cnt].header = pose_stamped.header;

    if (SAVE_LOOP_PATH)
    {
        ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp * 1e9 << ",";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << ","
              << P.y() << ","
              << P.z() << ","
              << Q.w() << ","
              << Q.x() << ","
              << Q.y() << ","
              << Q.z() << ","
              << endl;
        loop_path_file.close();
    }
    //draw local connection
    if (SHOW_S_EDGE)
    {
        list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();
        for (int i = 0; i < 4; i++)
        {
            if (rit == keyframelist.rend())
                break;
            Vector3d conncected_P;
            Matrix3d connected_R;
            if((*rit)->sequence == cur_kf->sequence)
            {
                (*rit)->getPose(conncected_P, connected_R);
                posegraph_visualization->add_edge(P, conncected_P);
            }
            rit++;
        }
    }
    if (SHOW_L_EDGE)
    {
        if (cur_kf->has_loop)
        {
            //printf("has loop \n");
            KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);
            Vector3d connected_P,P0;
            Matrix3d connected_R,R0;
            connected_KF->getPose(connected_P, connected_R);
            //cur_kf->getVioPose(P0, R0);
            cur_kf->getPose(P0, R0);
            if(cur_kf->sequence > 0)
            {
                //printf("add loop into visual \n");
                posegraph_visualization->add_loopedge(P0, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));
            }
            
        }
    }
    //posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0), Q);

	keyframelist.push_back(cur_kf);
    publish();
	m_keyframelist.unlock();
}

状态估计

这是比较难的一部分,放在vins_estimator文件下:

SFM算法是一种基于各种收集到的无序图片进行三维重建的离线算法。在进行核心的算法structure-from-motion之前需要一些准备工作,挑选出合适的图片。下面通过匹配点求相机之间的旋转和平移矩阵。

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);
	//cout << "r " << endl << r << endl;
	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;

}

稀疏的3D点云:

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)
{
	feature_num = sfm_f.size();
	//cout << "set 0 and " << l << " as known " << endl;
	// have relative_r relative_t
	// intial two view
	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;
	//cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl;
	//cout << "init t_l " << T[l].transpose() << endl;

	//rotate to cam frame
	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];


	//1: trangulate between l ----- frame_num - 1
	//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1; 
	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);
	}
	//3: triangulate l-----l+1 l+2 ... frame_num -2
	for (int i = l + 1; i < frame_num - 1; i++)
		triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
	//4: 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);
	}
	//5: 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);
			//cout << "trangulated : " << frame_0 << " " << frame_1 << "  3d point : "  << j << "  " << point_3d.transpose() << endl;
		}		
	}

/*
	for (int i = 0; i < frame_num; i++)
	{
		q[i] = c_Rotation[i].transpose(); 
		cout << "solvePnP  q" << " i " << i <<"  " <<q[i].w() << "  " << q[i].vec().transpose() << endl;
	}
	for (int i = 0; i < frame_num; i++)
	{
		Vector3d t_tmp;
		t_tmp = -1 * (q[i] * c_Translation[i]);
		cout << "solvePnP  t" << " i " << i <<"  " << t_tmp.x() <<"  "<< t_tmp.y() <<"  "<< t_tmp.z() << endl;
	}
*/
	//full BA
	ceres::Problem problem;
	ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
	//cout << " begin full BA " << endl;
	for (int i = 0; i < frame_num; i++)
	{
		//double array for ceres
		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)
		{
			problem.SetParameterBlockConstant(c_rotation[i]);
		}
		if (i == l || i == frame_num - 1)
		{
			problem.SetParameterBlockConstant(c_translation[i]);
		}
	}

	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;
	}
	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();
		//cout << "final  q" << " i " << i <<"  " <<q[i].w() << "  " << q[i].vec().transpose() << endl;
	}
	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]));
		//cout << "final  t" << " i " << i <<"  " << T[i](0) <<"  "<< T[i](1) <<"  "<< T[i](2) << endl;
	}
	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]);
	}
	return true;

}

姿态估计:


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

。。。

后续逐渐增加原理推导。

本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)

【VINS-Fusion入门之二】基于优化的多传感器融合 的相关文章

  • 基于ROS搭建机器人仿真环境

    别人的发复现及经验 https blog csdn net qq 38620941 article details 125321347 gazebo默认仿真环境 1 gazebo models 是系统下gazebo放置模型库的默认位置 2
  • 嵌入式Linux系统开发笔记(十六)

    根文件系统rootfs启动验证测试 接下来我们使用测试一下前面创建好的根文件系统 rootfs xff0c 测试方法使用 NFS 挂载 6 1 检查是否在Ubuntu主机中安装和开启了NFS服务 xff08 特别注意 xff1a nfs 配
  • 安卓5.0以上7.0以下使用Termux

    参考 xff1a https zhuanlan zhihu com p 400507701 说明 xff1a Termux支持5 0以上的安卓系统 Termux7 3版本之后 xff0c 仅支持7 0以上的安卓系统 1 安装Termux 设
  • 关于DSP的CCS6.0平台下的工程搭建(完全可移植)

    本工程以CCS6 0下新建TMS320F28335工程为例 xff0c 其他系列处理器工程搭建类似 xff0c 参考本例即可 工程搭建用到的F2833x TI官方库文件 下载链接 也可直接参考笔者搭建好CCS6 0的工程 下载链接 所建工程
  • STM32Fxx JTAG/SWD复用功能重映射

    问题描述 xff1a 在实验室调车过程中 xff0c 遇到的一个问题 xff1a 为了每次下载程序方便 xff0c 队员们往往会把 Jlink 插在板子上 xff0c 可是在调车过程中发现 xff0c 有时程序会莫名死掉 xff0c 而同样
  • VS2012编译RTKLIB——GNSS定位开源库

    RTKLIB 开源库 有着强大的 GPS 数据实时和后处理功能 xff0c 由于 笔者的毕业设计中需要对GPS 载波相位观测量进行 RTK 解算 xff0c 故而 xff0c 对 RTKLIB 开源库进行了学习与研究 RTKLIB 提供了很
  • 51单片机串行口波特率计算

    1 工作方式介绍 xff1a 方式 0 xff1a 这种工作方式比较特殊 xff0c 与常见的微型计算机的串行口不同 xff0c 它又叫 同步移位寄存器输出方式 在这种方式下 xff0c 数据从 RXD 端串行输出或输入 xff0c 同步信
  • 数码管显示问题总结

    1 数码管显示原理 我们最常用的是七段式和八段式 LED 数码管 xff0c 八段比七段多了一个小数点 xff0c 其他的基本相同 所谓的八段就是指数码管里有八个小 LED 发光二极管 xff0c 通过控制不同的 LED 的亮灭来显示出不同
  • 单片机与嵌入式linux 比较

    MCU门槛低 xff0c 入门容易 xff0c 但是灵活 xff0c 其实对工程师的软硬件功底要求更高 xff0c 随着半导体的飞速发展 xff0c MCU能实现很多匪夷所思匪夷所思的功能 xff0c 比如 xff0c 使用GPIO模拟1个
  • rtk 精确定位 简介

    RTK又称载波相位差分 xff1a 基准站通过数据链及时将其载波观测量及站坐标信息一同传送给用户站 用户站接收GPS卫星的载波相位与来自基准站的载波相位 xff0c 并组成相位差分观测值进行及时处理 xff0c 能及时给出厘米级的定位结果
  • STM32开发利器:STM32CubeMX

    这篇博客篇幅不长 xff0c 主要是为大家介绍ST公司推出的STM32CubeMX开发工具 xff0c 当成下周更新STM32 10个项目工程的预备篇 xff0c 同时FPGA FPGA 20个例程篇 xff1a 8 SD卡任意地址的读写
  • ROS命名空间

    ROS命令空间是一个很重要的内容 xff0c 官方文档 xff1a http wiki ros org Names 分为三类 xff1a relative xff0c global xff0c private 下边是一个官网给的示例 Nod
  • STM32CubeMX关于添加DSP库的使用

    前言 人生如逆旅 xff0c 我亦是行人 一 介绍 STM32 系列基于专为要求高性能 低成本 低功耗的嵌入式应用专门设计的 ARM Cortex M3 内核 而 DSP 应该是 TMS320 系列 xff0c TMS320 系列 DSP
  • STM32H750VBT6的DSP使用的学习——基于CubeMX

    前言 人生如逆旅 xff0c 我亦是行人 1 STM32H7的DSP功能介绍 xff08 STMicroelectronics xff0c 简称ST xff09 推出新的运算性能创记录的H7系列微控制器 新系列内置STM32平台中存储容量最
  • ROS中激光雷达数据类型传递转换及自定义点云数据类型介绍

    目录 一 ROS中激光雷达数据类型传递转换 xff1b 二 点云数据解析 三 自定义点云数据类型 一 ROS中激光雷达数据类型传递转换 xff1b ROS中涉及激光雷达传递的消息类型有两种 xff0c 一种是针对2D雷达 sensor ms
  • C/C++优秀书籍清单

    转载自 xff1a https www cnblogs com kimiway p 3225767 html 书籍是码农进步的阶梯 读好书 好读书 干一行爱一行 除了工作还有生活 在陪伴家人同时 也不忘提高自己 为更好的生活努力 1 C程序
  • 打印_battery_status.scale

    在px4的姿态控制中 xff0c publish控制量时代码乘以了一个 battery status scale xff0c scale effort by battery status if params bat scale en amp
  • 无名飞控

    无名飞控Time c文件 由于 无名飞控主要核心 xff1a 传感器滤波 姿态解算 惯导 控制等代码在TIME c里面运行 xff0c 所以先来分析这个文件 打开文件第一个函数 xff1a void Timer4 Configuration
  • 无名飞控姿态解算和控制(一)

    无名飞控的姿态解算和控制 从imu和磁力计 xff0c 气压计拿到数据后 xff0c 进入AHRSUpdate GraDes Delayback函数 xff0c 其中X w av Y w av Z w av来自陀螺仪 xff0c X g a
  • 无名飞控姿态解算和控制(三)

    继续码代码 上一篇主要写了自稳模式下的代码流程 xff0c 这次主要是飞控的定高和定点控制流程 首先是定高 控制模式在Main Leading Control里选择 定高模式代码 xff1a else if Controler Mode 6

随机推荐

  • 无名飞控框架梳理

    打开飞控的main c文件 首先是HardWave Init 飞控板内部资源 相关外设初始化 打开 include 34 Headfile h 34 Sensor Okay Flag Sensor Init Flag void HardWa
  • 无名飞控的时钟和延时

    首先是飞控里面调用了 SystemInit 时钟初始化这个里面 void SystemInit void Reset the RCC clock configuration to the default reset state for de
  • 谈谈bit位序的问题

    Linux内核里面有下面代码 struct iphdr if defined LITTLE ENDIAN BITFIELD u8 ihl 4 version 4 elif defined BIG ENDIAN BITFIELD u8 ver
  • 无名飞控的姿态解算和控制(四)

    上面几篇帖子已经写完控制流程还剩一点没说 if PPM Isr Cnt 61 61 100 PPM接收正常才进行传感器标定检测 Accel Calibration Check 加速度标定检测 Mag Calibration Check 磁力
  • 无名飞控的自抗扰控制

    无名飞控的自抗扰控制 自控制的算法可以见韩京清先生的书点击打开链接 为了自抗扰控制买了无名飞控 xff0c 现在看看它的自抗扰代码 首先初始化 xff1a 从代码上大致可以看出只对俯仰 横滚方向姿态内环角速度控制器采用ADRC自抗扰控制器
  • ARM寄存器与汇编指令详解

    介绍ARM寄存器之前 xff0c 先来介绍一下ARM处理的模式 xff1a 用户模式 User ARM处理器正常的程序执行状态 快速中断模式 FIQ 用于高速数据传输或通道处理 外部中断模式 IRQ 用于通用的中断处理 管理模式 Svc 操
  • STM32定时器---正交编码器模式详解

    编码器分类 xff1a 按工作原理 xff1a 光电式 磁电式和触点电刷式 按码盘的刻孔方式 xff1a 增量式和绝对式两类 由于博主接触面还不是很广 xff0c 一共就用过两个种类的编码器 xff0c 都是属于光电的 差分编码器 一般由8
  • VM虚拟机下给Ubuntu 目录分区增加容量的方法

    最近在编译androdi5 1代码的时候突然发现虚拟机容量不够了 xff0c 很是蛋疼 xff0c 只好摸索如何想办法给相应目录增加容量 xff0c 以下方法亲测可行 xff01 1 第一步当然是增加硬盘容量了 xff0c 这个需要用到VM
  • 1.uCOS-II简介及移植uCOS-II到STM32F103平台详细步骤

    I 说明 作者 xff1a WXP 翱翔云端的鸟 联系方式 328452854 64 qq com 13100610853 联系请注明 CSDN 申明 个人原创 xff0c 转载请先经过本人同意 xff01 要说的话 个人水平有限 写之前也
  • 1.nRF52832裸机教程--开发环境搭建

    I 说明 作者 xff1a WXP 翱翔云端的鸟 联系方式 328452854 64 qq com 13100610853 联系请注明CSDN 申明 个人原创 xff0c 转载请先经过本人同意 xff01 要说的话 个人水平有限 写之前也看
  • 3.nrf52832裸机教程--系统时钟

    I 说明 作者 xff1a WXP 翱翔云端的鸟 联系方式 328452854 64 qq com 13100610853 联系请注明CSDN 申明 个人原创 xff0c 转载请先经过本人同意 xff01 要说的话 个人水平有限 写之前也看
  • Docker中使用ROS-GUI

    Docker中使用ROS GUI比较麻烦 xff0c 好在有国外的大神解决了这个难题 下面 xff0c 我就教大家如何在Docker中使用ROS GUI 1拉取大神编写的镜像 docker pull ct2034 vnc ros kinet
  • Linux环境下搭建git服务器和TortoiseGit客户端 ssh key测试

    Linux环境下搭建git服务器和TortoiseGit客户端 ssh key测试 1 git的安装2 用户和 ssh目录创建3 本地创建公钥私钥4 在服务器端导入本地公钥5 开启服务器中的RSA认证6 创建仓库7 本地克隆 1 git的安
  • RealSense技术在SR300摄像头上的应用

    RealSense技术在SR300摄像头上的应用 一 实感摄像头 1 RealSense技术 在计算机的发展过程中我们始终没有抛弃键盘和鼠标 xff0c 前几年win8和触屏的配合形成 了一种新的电脑使用模式 xff0c 但是依然没有打破传
  • 自动控制理论(2)——控制系统的数学模型(微分方程、传递函数)

    系列文章目录 自动控制理论 xff08 1 xff09 自动控制理论概述 自动控制理论 xff08 3 xff09 控制系统的数学模型 xff08 系统框图和信号流图 xff09 文章目录 系列文章目录一 线性系统的微分方程1 微分方程的建
  • Android 8.1共享系统代理中的热点(LineageOS15.1)

    https github com Mygod VPNHotspot 下载安装这个软件 xff0c 需要ROOT 开发者选项 xff1a 关闭WLAN硬件加速 该软件设置 xff1a 关闭IPV6 打开 修复DHCP 开启手机自带的热点 该软
  • 浏览器页面滚动条美化(样式)

    浏览器页面滚动条美化 xff08 样式 xff09 最近测试反应我们的产品在浏览器中当页面宽高出现溢出的情况下页面滚动条太丑了 xff01 让我们美化一下 xff01 然后花了一点时间专研了一下关于滚动条样式的相关知识 xff0c 今天就在
  • Android串口工具

    参考Android系统实现方式 xff0c 串口操作采用native实现 xff0c 比java层用文件流的方式更高效 xff0c 不需要延时和睡眠处理 xff0c 大量自测不会出现读取不到数据等问题 特点 xff1a 1 提供基本的串口操
  • 【VINS-Fusion入门之一】让系统跑起来

    文章目录 简介配置执行单目 43 IMU双目 43 IMU双目相机双目 43 GPS 落地备注 xff1a 简介 VINS xff0c 英文缩写Visual Inertial Systems 是一个实时视觉SLAM框架 xff0c 2017
  • 【VINS-Fusion入门之二】基于优化的多传感器融合

    文章目录 简介特征参考论文 解读系统框图相机模型 xff1a 配置文件全局优化闭环优化状态估计 简介 VINS Fusion is an optimization based multi sensor state estimator whi