livox

开源代码位于：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& pData, const Eigen::Matrix3d& inner,Eigen::Matrix3d& Rot, Eigen::Vector3d& trans)
{
    vector pts_3d;
    vector 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<(0,0),t.at(1,0),t.at(2,0);

    std::cout<<"R = "< 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<

4. 迭代求解

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

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

欢迎分享，转载请注明来源：内存溢出

原文地址: https://outofmemory.cn/langs/634466.html