1. 初始化的流程
初始化采用的是视觉和IMU松耦合的方案首先用SFM求解滑动窗口内所有帧的位姿，和所有路标点的3D位置，然后跟IMU预积分的值对齐，求解重力向量，尺度因子，陀螺仪bias及每一帧对应的速度。
画一下初始化的流程图：
在看程序之前，先来看看这里用到的数据结构：
ImageFrame imageframe(image, header.stamp.toSec());
//tmp_pre_integration 是在processIMU() 时进行的 IntegrationBase *tmp_pre_integration;
imageframe.pre_integration = tmp_pre_integration;
//存储所有的imageFrame map<double, ImageFrame> all_image_frame;
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
初始化的入口，我们暂时只看初始化部分，至于初始化完成之后的操作，后面再说。
要进行初始化的条件是：（1）solver_flag == INITIAL（2）frame_count == WINDOW_SIZE（3）外参初始化成功，并且距离上一次初始化的时间大于0.1
//初始化
if (solver_flag == INITIAL)
{
if (frame_count == WINDOW_SIZE) //WINDOW_SIZE=10
{
bool result = false;
//外参初始化成功
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
//对视觉个惯性单元进行初始化
result = initialStructure();
//初始化时间戳更新
initial_timestamp = header.stamp.toSec();
}
}
}
2. initialStructure
由于代码比较长，我们按功能将它切分成几块。
（1）计算IMU的激励是否足够
/**************
1.1 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;
}
/**
* 方差公式：s^2 = [(x1 - x)^2 + (x2 - x)^2 + ... + (xn - x)^2]/n
* 标准差： s = sqrt(s^2)
*/
var = sqrt(var / ((int)all_image_frame.size() - 1));
//ROS_WARN("IMU variation %f!", var);
//通过加速度标准差判断IMU是否由充分运动，标准差必须大于等于0.25
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
}
}
这部分的代码比较简单，就是先求出来所有帧的平均加速度，然后再求加速的方差，然后求标准差。
（2）将feature队列，放到vector<SFMFeature>中，其中SFMFeature中的observation存放的是观测到的路标点的id和特征点坐标。如下图所示：
代码：
// ********* global sfm ********
Quaterniond Q[frame_count + 1];
Vector3d T[frame_count + 1];
map<int, Vector3d> sfm_tracked_points;//用于存储sfm重建出来的特征点坐标，所有观测到该特征点的图像的id和图像坐标
vector<SFMFeature> sfm_f;
/**
* 1.2 将f_manager.feature中的 feature存储到 sfm_f 中
*/
for (auto &it_per_id : f_manager.feature)// list<FeaturePerId> 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
//遍历每一个能观察到该feature的frame
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;//后面要++，前面为啥要-1呢？ 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);
}
（3）relativePose
Matrix3d relative_R;
Vector3d relative_T;
int l;
//通过求解本质矩阵来求解位姿
/**
* l 表示滑动窗口中第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧
*/
if (!relativePose(relative_R, relative_T, l)) //计算的结果给relative_R, relative_T
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
我们来仔细看看relativePose的代码
/**
* 这里的第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧
* 会作为参考帧到下面的全局sfm使用，得到的 R t 为当前帧到第l帧的坐标变换 R t
* 该函数判断滑动窗口中第一帧到最后一帧，对应特征点的平均视差大于30，且内点数大于12的帧，此时可进行初始化，同时返回当前帧到第l帧的坐标变化 R t
*/
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;
//寻找第 i 帧 到最后一帧对应的特征点，存放在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++)
{
//第j个对应点在第 i 帧和最后一帧的（x,y）
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());
//判断是否满足初始化条件：视差> 30， 内点数>12 solveRelativeRT 返回true
//solveRelativePoseRtT() 通过基础矩阵计算当前帧与第l帧的 R 和 T ，并判断内点数是否足够
//同时返回窗口最后一帧（当前帧）到第l帧（参考帧）的relative_R 和relativeT
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;
}
我们来理以下这个代码执行的过程：
1）从滑窗的第一帧开始，找和最后一帧的匹配点
2）如果匹配的点数大于20，则计算平均视差
3）如果视差满足要求，并且solveRelativeRT返回true，输出相关信息，到此结束
4）如果所有的帧都不能满足要求，就返回false
（4）getCorresponding
//找出两帧之间的共视特征点
vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
vector<pair<Vector3d, Vector3d>> corres;//存放特征点对
//遍历feature list中的每一个特征点
for (auto &it : feature) //list<FeaturePerId> feature
{
//endFrame = start_frame + feature_per_frame.size() - 1
if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
{
Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
//计算传入的frame_count_l 和 frame_count_r 距离最开始一帧的距离，也就是最新两帧的index
int idx_l = frame_count_l - it.start_frame;
int idx_r = frame_count_r - it.start_frame;
a = it.feature_per_frame[idx_l].point;
b = it.feature_per_frame[idx_r].point;
corres.push_back(make_pair(a, b));
}
}
//corres中存放的就是每一个特征it在两帧中对应的point
return corres;
}
这部分的代码挺简单的，注释也挺全的，就不在这里啰嗦了。
(5)solveRelativeRT
这个函数里主要就是调用了openCV库的函数findFundamentalMat来计算fundamental matrix，然后再用recoverPose根据fundamental matrix恢复相机的位姿。这里会得到一个当前帧到参考帧的旋转和平移。
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;
/**
* 求本质矩阵
* Mat cv::findFundamentalMat(
* InputArray points1, //第一幅图像 中的特征点数组
* InputArray points2, //第二幅图像中的特征点数组
* int method = FM_RANSAC, //计算基础矩阵的方法，这里使用的是8点法
* double ransacReprojThreshold = 3, //点到对极线的最大距离，超过这个值的点将被舍弃
* double confidence = 0.99, //矩阵正确的可信度
* OutputArray mask = noArray(), //输出在计算过程中没有被舍弃的点
*)
*/
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 cv::recoverPose(
* InputArray E, //本质矩阵
* InputArray points1, //第一幅图像点的数组
* InputArray points2, //第二幅图像点的数组
* InputArray cameraMatrix, //相机的内参矩阵
* OutputArray R, //第一帧坐标系到第二帧坐标系的旋转
* OutputArray t, //第一帧坐标系到第二帧坐标系的平移
* InputOutputArray mask = noArray() //在 findFundamentalMatrix() 中被丢弃的点
*)
*/
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;
}
这里有个问题，看recoverPose要使用的是essential matrix，而这里传入的确实fundamental matrix，是因为这里用的是归一化的坐标，所以相机的内参矩阵用的是单位阵，在这样的情况下，essential matrix 是等于 fundamental matrix的。recoverPose()参考博客：https://blog.csdn.net/u011341856/article/details/104116263
3. sfm.construct
三角化特征点，对滑窗的每一帧求解sfm问题。
GlobalSFM sfm;
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;
}
如果失败就marg掉old帧。下面来看看这个construct函数，函数有点长，咱们分段来看。
(1)把第l帧作为参考帧，计算最新一帧在参考帧的坐标系下的位姿。
feature_num = sfm_f.size();
//cout << "set 0 and " << l << " as known " << endl;
// have relative_r relative_t
// intial two view
//q为四元数数组，大小为frame_count+1 第l帧为参考帧
q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
//T 是平移数组
T[l].setZero();
//往q中存入最新帧的旋转，该旋转等于第 l 帧的旋转和 相对旋转的四元数乘积yy
q[frame_num - 1] = q[l] * Quaterniond(relative_R);// relative_R 两帧之间的旋转，通过 relativePose 计算出来的
T[frame_num - 1] = relative_T;
（2）创建数组，存储滑窗内第l帧到其他帧的旋转和平移
这里的旋转和平移都保存了两个方向的
//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]);
//将第l帧的旋转和平移存到pose当中
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_Quat存放q[l]的逆
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]);
/**
* matrix.block(i,j,p,q): 从 i，j开始，返回 p*q个元素，返回这个矩阵，原矩阵不变
* matrix.block<p,q>(i,j): 可以理解为一个p*q的子矩阵，原矩阵不变
*/
//将最新帧的旋转和平移量存入到 pose 中
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];
（3）对参考帧到当前帧的每一帧，三角化参考帧到该帧的路标点
//1: trangulate between l ----- frame_num - 1
for (int i = l; i < frame_num - 1 ; i++)
{
if (i > l)
{... ...}
// triangulate point based on the solve pnp result
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}
- 首先：对参考帧和当前帧直接三角化路标点。
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)
{
Vector3d point_3d;
triangulatePoint(Pose0, Pose1, point0, point1, point_3d);//根据位姿和归一化坐标，输出在参考坐标系下的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 : " << frame1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}
}
来看看这个三角化的函数是怎么实现的。
关于三角化的推导，请参考链接：https://blog.csdn.net/jsf921942722/article/details/95511167， 或者十四讲。
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);
}
有一个常识是我不知道原因的，为什么最小奇异值对应的向量即为所需的解？ 我找到了一个证明过程因为用的齐次坐标，所以解得的点有4维，齐次坐标的最后一维一定是为1的，所以在这里就是/ triangulated_point(3)。
- 然后：对参考帧和当前帧-1的帧，先用PnP求解相对参考帧的位姿，然后和最新帧进行三角化路标点。
//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];
//已知第i帧上出现的一些特征点的l系上的空间坐标，通过上一帧的旋转平移得到下一帧的旋转平移
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];
}
//根据pnp的结果进行三角化
// triangulate point based on the solve pnp result
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}
现在的重点就在这个PnP的函数上了。
solveFrameByPnP，对这个函数也按功能分成一段一段的
- 查找第j帧三角化过，并且在第i帧也出现过的点，push到对应的容器中。
vector<cv::Point2f> pts_2_vector;
vector<cv::Point3f> pts_3_vector;
for (int j = 0; j < feature_num; j++)//feature_num = sfm_f.size();
{
//如果没有三角化的点，不对这个点进行操作
if (sfm_f[j].state != true)
continue;
Vector2d point2d;
//依次遍历特征j在每一帧中的归一化坐标
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
{
//frame_id == i 如果该特征在i帧上出现过
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);//把在待求帧i上的归一化坐标放到容器中
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);//把对应的3d坐标放在容器中
break;
}
}
}
- 如果匹配的点数小于10，就初始化失败了
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;
}
- 调用openCV的函数，进行PnP的求解 Opencv:SolvePNP：https://www.jianshu.com/p/b97406d8833c
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;
//第i帧到第l帧的旋转和平移
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;
（4）从第 l+1帧到滑窗的最后一帧，再与第 l 帧进行三角化补充3D坐标
上一步求出了 第l帧 后面每一帧相对于第l帧的位姿， 也求出了他们相对于最后一帧的共视点的3D坐标，但是这是不够的，现在继续补充3D坐标，和第l帧进行三角化
//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);
（5）对 l 帧之前的每一帧，先用pnp求解位姿，然后再三角化。
这里的代码和之前的一样，就不解释了。
//4: solve pnp l-1; triangulate l-1 ----- l
// l-2 l-2 ----- l
//对于第一帧和参考帧之间的某一帧，先用PnP求解该帧的位姿，然后三角化该帧到参考帧的路标点
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）三角化其它未恢复位姿的点
恢复出来的3D坐标是在参考帧 l 帧的坐标系下的坐标
//5: triangulate all other points
for (int j = 0; j < feature_num; j++)
{
if (sfm_f[j].state == true)//true 三角化过了
continue;
if ((int)sfm_f[j].observation.size() >= 2)
{
Vector2d point0, point1;
int frame_0 = sfm_f[j].observation[0].first;//第一次观测的id
point0 = sfm_f[j].observation[0].second;//第一次观测的归一化坐标
int frame_1 = sfm_f[j].observation.back().first;//最后一次被观测的id
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;
}
}
（7）BA优化
a) 声明problem
ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
b) 加入待优化变量 这里仅添加了每一帧
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);
}
c）固定先验值
因为第l帧是参考系，最新帧的平移也是先验，如果不固定住，原本可观的量会变得不可观
if (i == l)
{
problem.SetParameterBlockConstant(c_rotation[i]);
}
if (i == l || i == frame_num - 1)
{
problem.SetParameterBlockConstant(c_translation[i]);
}
d) 加入残差块
最小化重投影误差，需要2D->3D的信息
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);
}
}
e）舒尔消元
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;
}
f）返回参考帧 l 帧坐标系下的 3D 坐标和优化后的全局位姿
这里优化得到的是第l帧坐标系到各帧之间的旋转和平移，应将其转换为每一帧在第l帧坐标系下的坐标
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;
优化完成后，需要获取各帧在 l 系下的位姿，所以需要inverse操作，然后把特征点在 l 帧下的3D坐标传递出来。
这部分实在是太长了，我需要把它分成两部分来写，这一部分就先到这里吧。
这部我参考了博客：https://blog.csdn.net/iwanderu/article/details/104728668/
和崔华坤的VINS论文推导及代码解析，链接在前面的文章已经给过了，就不再重复了。
476
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
