livox_lidar_camera_calibration学习--标定外参求解

开源代码位于：GitHub - Shelfcol/livox_camera_lidar_calibration_modified: livox_camera_lidar_calibration的改进

原始的代码里面需要给定初始的旋转平移矩阵参数，然后使用ceres优化对应点的投影误差，再去判断重投影误差的大小。这里进行改进，可以不给定初始参数进行优化求解，然后剔除重投影误差大的对应点，再次优化。

roslaunch camera_lidar_calibration getExt_modified.launch

1. 读取lidar和camera的对应点，内参信息

2. 使用opencv自带的函数求解位姿初值

cv::solvePnP

这里遇到一个bug，求解出来的平移矩阵差了5米，还需要再看一下。

void PnpTrans(const vector<PnPData>& pData, const Eigen::Matrix3d& inner,Eigen::Matrix3d& Rot, Eigen::Vector3d& trans)
{
    vector<cv::Point3d> pts_3d;
    vector<cv::Point2d> pts_2d;
    for_each(pData.begin(),pData.end(),[&](auto a){
        pts_3d.emplace_back(a.x,a.y,a.z);
        pts_2d.emplace_back(a.u,a.v);
    });
    cv::Mat K(2, 3, CV_64F);
    cv::eigen2cv(inner,K);
    cv::Mat r,t;
    cv::solvePnP(pts_3d,pts_2d,K,cv::Mat(),r,t);
    cv::Mat R;
    cv::Rodrigues(r,R);

    cv::cv2eigen(R,Rot);
    cv::cv2eigen(t,trans);
    // trans<<t.at<double>(0,0),t.at<double>(1,0),t.at<double>(2,0);

    std::cout<<"R = "<<R<<std::endl;
    std::cout<<"t = "<<t<<std::endl;
    std::cout<<"R = "<<Rot<<std::endl;
    std::cout<<"t = "<<trans<<std::endl;
}

3. ceres优化求解

这里为了降低外点影响，ceres使用HuberLoss函数。

对于对应点 u,P，内参K，外参R，t，重投影误差为

, 

class external_cali {
public:
    external_cali(PnPData p) {
        pd = p;
    }

    template <typename T>
    bool operator()(const T *_q, const T *_t, T *residuals) const {
        Eigen::Matrix<T, 3, 3> innerT = inner.cast<T>();
        Eigen::Quaternion<T> q_incre{_q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ]};
        Eigen::Matrix<T, 3, 1> t_incre{_t[ 0 ], _t[ 1 ], _t[ 2 ]};

        Eigen::Matrix<T, 3, 1> p_l(T(pd.x), T(pd.y), T(pd.z));
        Eigen::Matrix<T, 3, 1> p_c = q_incre.toRotationMatrix() * p_l + t_incre;
        Eigen::Matrix<T, 3, 1> p_2 = innerT * p_c;

        residuals[0] = p_2[0]/p_2[2] - T(pd.u);
        residuals[1] = p_2[1]/p_2[2] - T(pd.v);

        return true;
    }

    static ceres::CostFunction *Create(PnPData p) {
        return (new ceres::AutoDiffCostFunction<external_cali, 2, 4, 3>(new external_cali(p)));
    }

private:
    PnPData pd;
};

求解出转换矩阵之后，求解重投影误差

遍历所有点，构建ceres问题，优化求解q,t

ceres::LocalParameterization * q_parameterization = new ceres::EigenQuaternionParameterization();
ceres::Problem problem;

problem.AddParameterBlock(ext, 4, q_parameterization);
problem.AddParameterBlock(ext + 4, 3);

for(size_t j=0; j< pData.size(); ++j) {
    if(mask[j]==1)  {continue;} // 跳过误差大的点
    ceres::CostFunction *cost_function;
    cost_function = external_cali::Create(pData[j]);
    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(5), ext, ext + 4);
}

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout << summary.BriefReport() << endl;

Eigen::Matrix3d rot = m_q.toRotationMatrix();
writeExt(extrinsic_path, rot, m_t);
cout << rot << endl;
cout << m_t << endl;

计算重投影误差，将误差大于阈值的对应的mask置为1

cout << "Use the extrinsic result to reproject the data" << endl;
float error[2] = {0, 0};
vector<float> extrinsic{(float)rot(0,0),(float)rot(0,1),(float)rot(0,2),(float)m_t(0),
                        (float)rot(1,0),(float)rot(1,1),(float)rot(1,2),(float)m_t(1),
                        (float)rot(2,0),(float)rot(2,1),(float)rot(2,2),(float)m_t(2)};
getUVError(intrinsic, extrinsic,  pData,  error, error_threshold,mask);
std::cout<<"mask: ";
for_each(mask.begin(),mask.end(),[&](auto& a){ std::cout<<a<<"";});
std::cout<<std::endl;
cout << "u average error is: " << error[0] << endl;
cout << "v average error is: " << error[1] << endl;
if (error[0] + error[1] < error_threshold) {
    cout << endl << "The reprojection error smaller than the threshold, extrinsic result seems ok" << endl;
}
else {
    cout << endl << "The reprojection error bigger than the threshold, extrinsic result seems not ok" << endl;
}

4. 迭代求解

将重投影误差大的对应点去除，重新回到步骤3进行求解。

扫描二维码关注公众号，回复： 16623003 查看本文章

5. 优化结果和原版代码对比

原本结果：

u average error is: 3.83361
v average error is: 4.05955

改进结果：

u average error is: 2.68588
v average error is: 2.65495