2d激光雷达（rplidar_s1）与双目摄像头联合标定

前段时间由于项目需要使用摄像头（realsense d435i）与单线激光雷达进行融合，于是就对这两个传感器进行了标定，使用的是CamLaserCalibraTool ，这是别人开源的一个工具，使用教程如下

https://github.com/llailinn/CamLaserCalibraTool

1. 首先安装并编译整个工具，具体方法见github中记录的方法
2. 然后需要修改config文件夹中对应的配置文件（设置相机模型，相机与激光雷达的话题）
3. 在摄像头时没有遇到什么问题
4. 激光时，结果发现没办法识别到我们的标定板（识别成功会标成红色）,这部分的代表在目录下的src/ 目录下的文件

cv::Vec3b color_value(0,0,255);  

5. 重点关注这个函数，把这个函数搞懂，就明白了这个标定的原理了

AutoGetLinePts（）  //提取标定板的线 原型如下
// 直接从每一帧激光的正前方开始搜索一定距离范围内的符合平面标定板形状的激光线段
    int n = points.size();  //雷达一圈的数据为720个
    //int id = n/2;   //可以设置扫描的开始点
    int id = 0.5*n; 
//        std::cout << points.at(id).transpose() <<" "< segs;
    double dist_thre =  0.05 ; //0.05;
    int skip = 1; //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) {
    for (int i = 0; i < 100; i += skip) {
        Eigen::Vector3d pt = points[i];
        int col = (int)(pt.x() / z * focal + img_w/2);
        int row = (int)(- pt.y() / z * focal + img_w/2);  // -Y/Z 加了一个负号, 是为了抵消针孔投影时的倒影效果

        if(col > img_w-1 || col< 0 || row > img_w-1 || row < 0)
            continue;

        cv::Vec3b color_value(255,0,0);
        img.at(row, col) = color_value;

        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     激光肯定小于100m
        {
            if(fabs(d1-d2) < dist_thre)  //  8cm    //dist_thre
            {
                seg.id_end = nextPt;

            } else
            {
                newSeg = true;
                Eigen::Vector3d dist = points.at(seg.id_start) - points.at(seg.id_end);
                //std::cout <<"dist: "<< 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 > 30   )  //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;
        }
    }