# [踩坑记录]rplidar A1与乐视LeTMC-520摄像头联合标定中遇到的问题

[踩坑记录]rplidar A1与乐视LeTMC-520摄像头联合标定中遇到的问题

本人是初学者，手里有一个已经安装了思岚rplidar A1的小车，试图再装一个乐视LeTMC-520摄像头，通过标定外参，将二者的点云对齐。（使用的标定工具是此文中介绍的CamLaserCalibraTool）

首先使用ROS中的camera-calibration包对rgb摄像头和深度摄像头进行了标定，（没有做rgb和depth图像配准）接着按照上面的博客中介绍的方法，使用雷达和摄像头同时扫描标定板，录制rosbag数据包。当进行到联合标定的第二步，启动calibra_offline.launch 文件时，标定工具无法检测到落在标定板上的激光点。
粗略地阅读代码发现问题在src/selectScanPoints.cpp文件中的AutoGetLinePts函数，该函数的功能为在相机正前方一定角度内搜索落在标定板上的激光点，并将激光点返回。具体做法是设一帧激光有n个点，以序号为n/2的雷达点为正前方，根据雷达扫描的间隔角度和左右最大可视角度计算左右雷达点的序号，然后遍历左右序号之间的点，再搜索直线，代码如下。

/// detect and get line
    // 直接从每一帧激光的正前方开始搜索一定距离范围内的符合平面标定板形状的激光线段
    int n = points.size();
    int id = n/2;
//        std::cout << points.at(id).transpose() <<" "< segs;
    double dist_thre = 0.05;
    int skip = 3;
    int currentPt = id_right;
    int nextPt = currentPt + skip;
    bool newSeg = true;
    LineSeg seg; 
    for (int i = id_right; i < id_left - skip; i += skip) {

        if(newSeg)
        {
            seg.id_start = currentPt;
            seg.id_end = nextPt;
            newSeg = false;
        }

        double d1 = points.at(currentPt).head(2).norm();
        double d2 = points.at(nextPt).head(2).norm();
        double range_max = 100;
        if(d1 < range_max && d2 < range_max)    // 有效数据,  激光小于 100 m
        {
            if(fabs(d1-d2) < dist_thre)  //  8cm
            {
                seg.id_end = nextPt;

            } else
            {
                newSeg = true;
                Eigen::Vector3d dist = points.at(seg.id_start) - points.at(seg.id_end);
                if(dist.head(2).norm() > 0.2
                   && points.at(seg.id_start).head(2).norm() < 2
                   && points.at(seg.id_end).head(2).norm() < 2
                   && seg.id_end-seg.id_start > 50   )  // 至少长于 20 cm, 标定板不能距离激光超过2m, 标定板上的激光点肯定多余 50 个
                {
                    seg.dist = dist.head(2).norm();
                    segs.push_back(seg);
                }
            }

            currentPt = nextPt;        // 下一个点
            nextPt = nextPt + skip;
        } else
        {
            if(d1 > range_max)
            {
                currentPt = nextPt;        // 下一个点
            }
            nextPt = nextPt + skip;
        }
    }

但是来看rplidar A1的坐标系图：

它是从x周相反方向开始扫描的，此时激光点序号为零，根据雷达的安装方向，序号为n/2的雷达点变成了小车的正后方，所以无法检测到标定板。如果希望检测小车前方120度范围内有没有标定板，那么应该遍历的雷达点就应该是[300,359]和[0,60]。（扫描角度检测是1度，一帧360点），由于本人标定时前方比较空旷，所以直接简单粗暴地把视场内较远的点都丢掉了，只有落在标定板上的点被保留下来。下图红色线段为落在标定板上地点

标定后的效果

最后使用标定好的变换矩阵，将rgbd的点云变换到laser坐标系又出现问题，激光点落在了rgbd相机点云的后方：


出现这种情况，检查了相机安装位置，以及外参矩阵，怀疑为深度相机测得距离比实际小，将小车放在距离目标（上图所示）120cm左右的位置对目标进行观测，比较两者的数值，发现相机测得的深度在1.15m到1.16m之间，雷达测得距离为1.19m-1.20m左右，深度相机测得距离比实际距离短了，但具体原因不清楚，可能是标定质量太差、可能被摔坏了、不知道其他用这个相机的人碰到这种情况没有。