前段时间由于项目需要使用摄像头(realsense d435i)与单线激光雷达进行融合,于是就对这两个传感器进行了标定,使用的是CamLaserCalibraTool ,这是别人开源的一个工具,使用教程如下
https://github.com/llailinn/CamLaserCalibraTool
- 首先安装并编译整个工具,具体方法见github中记录的方法
- 然后需要修改config文件夹中对应的配置文件(设置相机模型,相机与激光雷达的话题)
- 在摄像头时没有遇到什么问题
- 激光时,结果发现没办法识别到我们的标定板(识别成功会标成红色),这部分的代表在目录下的src/ 目录下的文件
cv::Vec3b color_value(0,0,255);
- 重点关注这个函数,把这个函数搞懂,就明白了这个标定的原理了
AutoGetLinePts() //提取标定板的线 原型如下
// 直接从每一帧激光的正前方开始搜索一定距离范围内的符合平面标定板形状的激光线段
int n = points.size(); //雷达一圈的数据为720个
//int id = n/2; //可以设置扫描的开始点
int id = 0.5*n;
// std::cout << points.at(id).transpose() <<" "<<points.at(id+1).transpose() <<std::endl;
// 假设每个激光点之间的夹角为0.3deg,
// step 1: 如果有激光标定板,那么激光标定板必须出现在视野的正前方 120 deg 范围内(通常相机视野也只有 120 deg),也就是左右各 60deg.
//int delta = 60/0.3;
int delta = 80;
//int id_left = std::min( id + delta, n-1);
//int id_right = std::max( id - delta , 0);
int id_left = std::min( id + delta, n-1);
int id_right = std::max( id - delta , 0);
int dist_left = points.at(id_left).norm();
int dist_right = points.at(id_right).norm();
// 逻辑别搞复杂了。
std::vector<LineSeg> 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<cv::Vec3b>(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() <<std::endl;
std::cout <<"seg: "<< seg.id_end-seg.id_start <<std::endl;
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 > 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;
}
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)