VINS初始化之外参在线标定
程序员文章站
2022-03-07 11:45:55
...
VINS初始化之外参在线标定
文章目录
一、理论推导
估计旋转外参数Qbc具体公式推导:
二、代码实现
initial_ex_rotation.cpp
if (ESTIMATE_EXTRINSIC == 2)
{
cout << "calibrating extrinsic param, rotation movement is needed" << endl;
if (frame_count != 0)
{
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
// ROS_WARN("initial extrinsic rotation calib success");
// ROS_WARN_STREAM("initial extrinsic rotation: " << endl
// << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;
}
}
}
/**
* @Description: 在线标定相机到imu之间的相对旋转Rbc
* @param:corres: 两帧之间多个匹配点的归一化坐标
* @param:delta_q_imu: 两帧陀螺仪积分得到的相对旋转
* @return:calib_ric_result: 标定结果
*/
int flag = 0;
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
// std::cout << "frame_count"<<frame_count << std::endl;
Rc.push_back(solveRelativeR(corres));
Rimu.push_back(delta_q_imu.toRotationMatrix());
// std::cout << "ric" << ric << std::endl;
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);
// std::cout << "Rc: " <<Rc.at(flag)<< std::endl;
// std::cout << "Rc_g: " << Rc_g.at(flag)<< std::endl;
flag++;
Eigen::MatrixXd A(frame_count * 4, 4);
A.setZero();
int sum_ok = 0;
for (int i = 1; i <= frame_count; i++)
{
Quaterniond r1(Rc[i]);
Quaterniond r2(Rc_g[i]);
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
//ROS_DEBUG("%d %f", i, angular_distance);
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;//鲁棒核权重
++sum_ok;
Matrix4d L, R;
double w = Quaterniond(Rc[i]).w();
Vector3d q = Quaterniond(Rc[i]).vec();//x,y,z
L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
L.block<3, 1>(0, 3) = q;
L.block<1, 3>(3, 0) = -q.transpose();
L(3, 3) = w;
Quaterniond R_ij(Rimu[i]);
w = R_ij.w();
q = R_ij.vec();
R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
R.block<3, 1>(0, 3) = q;
R.block<1, 3>(3, 0) = -q.transpose();
R(3, 3) = w;
A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
}
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_R(x);
std::cout << "x: " << x << std::endl;
std::cout << "estimated_R" << estimated_R.coeffs() << std::endl;
ric = estimated_R.toRotationMatrix().inverse();
//cout << svd.singularValues().transpose() << endl;
//cout << ric << endl;
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
std::cout << "svd " << svd.singularValues() << std::endl;
std::cout << "ric_cov " << ric_cov << std::endl;
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) {
calib_ric_result = ric;
std::cout << "calib_ric_result: " << calib_ric_result << std::endl;
return true;
}
else
return false;
}
Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
std::cout <<"corres.size()"<< corres.size() << std::endl;
if (corres.size() >= 9) {
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)));
// std::cout << " ll : ******************" <<ll<< std::endl;
// std::cout << " rr :" << rr<< std::endl;
}
cv::Mat E = cv::findFundamentalMat(ll, rr);
// std::cout << "solve fundamental " << E << std::endl;
cv::Mat_<double> R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2);
if (determinant(R1) + 1.0 < 1e-09)
{
E = -E;
decomposeE(E, R1, R2, t1, t2);
}
double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;
Matrix3d ans_R_eigen;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
ans_R_eigen(j, i) = ans_R_cv(i, j);
return ans_R_eigen;
}
return Matrix3d::Identity();
}
double InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,
const vector<cv::Point2f> &r,
cv::Mat_<double> R, cv::Mat_<double> t)
{
cv::Mat pointcloud;
cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0),
R(1, 0), R(1, 1), R(1, 2), t(1),
R(2, 0), R(2, 1), R(2, 2), t(2));
cv::triangulatePoints(P, P1, l, r, pointcloud);
// std::cout << "pointcloud: " << pointcloud.size()<< std::endl;//150*4
int front_count = 0;
for (int i = 0; i < pointcloud.cols; i++)
{
double normal_factor = pointcloud.col(i).at<float>(3);
cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
// std::cout << "p_3d_l:" << p_3d_l << std::endl;//3*1
// std::cout << "p_3d_l(2)" << p_3d_l(2) << std::endl;
cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
if (p_3d_l(2) > 0 && p_3d_r(2) > 0)
front_count++;
}
std::cout << "front_count" << front_count << std::endl;
// ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols);
return 1.0 * front_count / pointcloud.cols;
}
void InitialEXRotation::decomposeE(cv::Mat E,
cv::Mat_<double> &R1, cv::Mat_<double> &R2,
cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
cv::SVD svd(E, cv::SVD::MODIFY_A);
cv::Matx33d W(0, -1, 0,
1, 0, 0,
0, 0, 1);
cv::Matx33d Wt(0, 1, 0,
-1, 0, 0,
0, 0, 1);
R1 = svd.u * cv::Mat(W) * svd.vt;
R2 = svd.u * cv::Mat(Wt) * svd.vt;
t1 = svd.u.col(2);
t2 = -svd.u.col(2);
/*******************验证视觉SLAM十四讲公式是否正确**************/
// cv::Mat_<double> t3;
// cv::Mat_<double> ut;
// cv::transpose(svd.u, ut);
// cv::Matx33d t(1, 0, 0, 0, 1, 0, 0, 0, 0);
// std::cout << " Wt: " <<t.col(0)<< std::endl;
// std::cout << "svd.u " << svd.u << std::endl;
// std::cout << "svd.vt" << svd.vt << std::endl;
// t3 = svd.u * cv::Mat(W) *cv::Mat(t)*ut;
// std::cout << " t1: " << t1 << std::endl;
// std::cout << " t: " << t3<< std::endl;
}