The open source code is located at: GitHub - Shelfcol/livox_camera_lidar_calibration_modified: Improvements to livox_camera_lidar_calibration

In the original code, the initial rotation and translation matrix parameters need to be given, and then ceres is used to optimize the projection error of the corresponding point, and then the size of the reprojection error is determined. To improve here, you can perform optimization without giving initial parameters, and then eliminate corresponding points with large reprojection errors and optimize again.

roslaunch camera_lidar_calibration getExt_modified.launch

1. Read the corresponding points and internal parameter information of lidar and camera

2. Use the function that comes with opencv to solve the initial value of pose

cv::solvePnP

I encountered a bug here. The calculated translation matrix is 5 meters off. I need to look at it again.

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 optimization solution

In order to reduce the influence of external points, ceres uses the HuberLoss function.

For the corresponding points u, P, internal parameters K, and external parameters R, t, the reprojection error is

$P' = RP+t$

$err = u-\frac{P'}{P'_z}$ ,

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

After solving the transformation matrix, solve for the reprojection error

Traverse all points, construct the ceres problem, and optimally solve 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;

Calculate the reprojection error and set the corresponding mask to 1 if the error is greater than the threshold.

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. Iterative solution

Remove the corresponding points with large reprojection errors and return to step 3 to solve.

5. Comparison between optimization results and original code

Original result:

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

Improved results:

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

livox_lidar_camera_calibration learning--Calibration external parameters solution