jzq
Gitee
commit
81512baac3
685
fusion.cpp
Normal file
685
fusion.cpp
Normal file
@ -0,0 +1,685 @@
|
|||||||
|
#include <set>
|
||||||
|
#include "fusion.h"
|
||||||
|
// 角度 -> 弧度转换
|
||||||
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
||||||
|
// fusion函数
|
||||||
|
char LCFusion::fusion(std::vector<Mat> &camInfo)
|
||||||
|
{
|
||||||
|
projectedPoints.clear();
|
||||||
|
if (camInfo.size() < 3) // 判断返回的双目测距信息是否为空，正常为3
|
||||||
|
{
|
||||||
|
std::cout << "There is no detection box!" << std::endl;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
Mat boxes = camInfo[1], dwha = camInfo[2];
|
||||||
|
projectedPoints.clear();
|
||||||
|
|
||||||
|
if (this->upScan.pointCloud.empty())
|
||||||
|
{
|
||||||
|
std::cout << "There is no lidar data!" << std::endl;
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (boxes.empty())
|
||||||
|
{
|
||||||
|
std::cout << "There is no detection box!" << std::endl;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
draw_point(camInfo); // 画出雷达映射到图像中的点
|
||||||
|
std::cout << "Fusion start!" << std::endl;
|
||||||
|
// 大物体
|
||||||
|
vector<int> clusterlable = clusters_bboxs(boxes); // 得到每簇雷达映射的检测框标签（检测框索引值）
|
||||||
|
optimize_clusterlable(clusterlable, boxes); // 优化检测框（针对镂空物体）
|
||||||
|
big_fusion(clusterlable, boxes, dwha); // 大物体赋语义
|
||||||
|
|
||||||
|
//小物体
|
||||||
|
small_box_without_dis(boxes, dwha); // 无距离信息的检测框映射
|
||||||
|
small_fusion(boxes, dwha); // 小物体赋语义
|
||||||
|
}
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::big_fusion(vector<int> &clusterlable, Mat &bboxs, Mat &dwha) // 大物体赋语义
|
||||||
|
{
|
||||||
|
std::cout << "big_fusion" << std::endl;
|
||||||
|
for (int i = 0; i < clusterlable.size(); i++) // 遍历每簇雷达
|
||||||
|
{
|
||||||
|
int bbox_num = clusterlable[i];
|
||||||
|
|
||||||
|
if (bbox_num != -1) // -1代表无检测框与之对应
|
||||||
|
{
|
||||||
|
int bbox_cls = bboxs.at<float>(bbox_num, 5);
|
||||||
|
if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14) // 排除扩散物体（风扇底座、吧台椅）
|
||||||
|
{
|
||||||
|
//std::cout << "bboxcls:" << bbox_cls << std::endl;
|
||||||
|
for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++) // 遍历一簇雷达的所有点赋语义
|
||||||
|
{
|
||||||
|
int laser_index = this->upScan.clustedCloud[i][j].at<float>(0, 3);
|
||||||
|
this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 4) // 扩散物体雷达点应该很少
|
||||||
|
{
|
||||||
|
//cout << "******start draw circle******" << endl;
|
||||||
|
double d = dwha.at<float>(bbox_num, 0);
|
||||||
|
if (d > 0)
|
||||||
|
{
|
||||||
|
draw_circle(i, bbox_num, bboxs, dwha);
|
||||||
|
} // 扩散物体赋语义
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void LCFusion::small_fusion(Mat &bboxs, Mat &dwha) // 小物体u赋语义
|
||||||
|
{
|
||||||
|
std::cout << "small_fusion" << std::endl;
|
||||||
|
for (int i = 0; i < bboxs.rows; i++)
|
||||||
|
{
|
||||||
|
int c = bboxs.at<float>(i, 5);
|
||||||
|
float x_mim = bboxs.at<float>(i, 0);
|
||||||
|
float y_min = bboxs.at<float>(i, 1);
|
||||||
|
float x_max = bboxs.at<float>(i, 2);
|
||||||
|
float y_max = bboxs.at<float>(i, 3);
|
||||||
|
std::cout << "cls:" << c << " xmin:" << x_mim << " ymin:" << y_min << " xmax:" << x_max << " ymax:" << y_max << std::endl;
|
||||||
|
if (class_index[c] == 0 || class_index[c] == 2 || (class_index[c] == 4 || class_index[c] == 5)) // 判断小物体u
|
||||||
|
{
|
||||||
|
|
||||||
|
Mat temp(2, 1, CV_32F);
|
||||||
|
double d = dwha.at<float>(i, 0);
|
||||||
|
double a = dwha.at<float>(i, 3); // 取距离和角度
|
||||||
|
if (d == -1)
|
||||||
|
continue;
|
||||||
|
//std::cout << "d:" << d << " a: " << a << std::endl;
|
||||||
|
temp.at<float>(0, 0) = float((dwha.at<float>(i, 0) / 100.0) * cos(dwha.at<float>(i, 3)));
|
||||||
|
temp.at<float>(1, 0) = float((dwha.at<float>(i, 0) / 100.0) * sin(dwha.at<float>(i, 3)));
|
||||||
|
temp = c2lR * temp + c2lT; // 相机坐标系-> 雷达坐标系转换
|
||||||
|
double y = temp.at<float>(0, 0);
|
||||||
|
double x = temp.at<float>(1, 0);
|
||||||
|
float dis = sqrt(x * x + y * y); // 转极坐标
|
||||||
|
|
||||||
|
float angle = 0.0; // 得到雷达点索引
|
||||||
|
if (y >= 0)
|
||||||
|
angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
|
||||||
|
double data_angle = atan((dwha.at<float>(i, 1)) / (200 * dis));
|
||||||
|
data_angle /= this->upScan.angleInc; // 由测距宽度信息得到需更改的雷达点数的一半
|
||||||
|
angle = (int)angle - (int)data_angle; // 起始雷达索引
|
||||||
|
int num_point = 2 * (int)data_angle + 1; // 更改的雷达点数
|
||||||
|
|
||||||
|
int len = this->upScan.distance.size();
|
||||||
|
for (int i = 0; i < num_point; i++)
|
||||||
|
{
|
||||||
|
if ((angle + i) < 0 || (angle + i) > (len - 1)) // 防止越界
|
||||||
|
continue;
|
||||||
|
|
||||||
|
this->upScan.distance[angle + i] = dis;
|
||||||
|
this->upScan.intensities[angle + i] = class_intensity[c + 1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (class_index[c] == 1 || class_index[c] == 3 ||class_index[c] == 6)
|
||||||
|
{
|
||||||
|
int len = this->upScan.distance.size();
|
||||||
|
Mat z = r->Z.clone();
|
||||||
|
if (countNonZero(z) < 1)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (z.cols >= 2)
|
||||||
|
{
|
||||||
|
int begin_index = 0;
|
||||||
|
for(int j = z.cols-1; j >=0; j-- )
|
||||||
|
{
|
||||||
|
int d = z.at<float>(0, j);
|
||||||
|
if (d != 0)
|
||||||
|
{
|
||||||
|
begin_index = j;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cout << "tizhongcheng " << z.cols << endl;
|
||||||
|
float d_begin = z.at<float>(0, begin_index)/1000.;
|
||||||
|
float a_begin = z.at<float>(1, begin_index);
|
||||||
|
cout << "d_begin: " << d_begin << " a_begin: " << a_begin << endl;
|
||||||
|
Mat temp(2, 1, CV_32F);
|
||||||
|
temp.at<float>(0, 0) = float(d_begin * cos(a_begin));
|
||||||
|
temp.at<float>(1, 0) = float(d_begin * sin(a_begin));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
double y = temp.at<float>(0, 0);
|
||||||
|
double x = temp.at<float>(1, 0);
|
||||||
|
float dis = sqrt(x * x + y * y);
|
||||||
|
float angle_index = 0.0; // 得到雷达点索引
|
||||||
|
if (y >= 0)
|
||||||
|
angle_index = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle_index = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
angle_index = (int)angle_index;
|
||||||
|
if (angle_index >= 0 && angle_index <len) // 防止越界
|
||||||
|
{
|
||||||
|
this->upScan.distance[angle_index] = dis;
|
||||||
|
this->upScan.intensities[angle_index] = class_intensity[c + 1];
|
||||||
|
}
|
||||||
|
cout << "dis: " << dis << " angle_index: " << angle_index << endl;
|
||||||
|
angle_index += 1;
|
||||||
|
|
||||||
|
for (int i = begin_index-1; i >= 0; i--)
|
||||||
|
{
|
||||||
|
float temp_d = z.at<float>(0, i)/1000.;
|
||||||
|
float temp_a = z.at<float>(1, i);
|
||||||
|
temp.at<float>(0, 0) = float(temp_d * cos(temp_a));
|
||||||
|
temp.at<float>(1, 0) = float(temp_d * sin(temp_a));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
y = temp.at<float>(0, 0);
|
||||||
|
x = temp.at<float>(1, 0);
|
||||||
|
dis = sqrt(x * x + y * y);
|
||||||
|
float temp_angle = 0.0; // 得到雷达点索引
|
||||||
|
if (y >= 0)
|
||||||
|
temp_angle = acos(x / dis);
|
||||||
|
else
|
||||||
|
temp_angle =2 * M_PI - acos(x / dis);
|
||||||
|
float angle_now = this->upScan.angleMin + this->upScan.angleInc*angle_index;
|
||||||
|
if (temp_angle >= angle_now)
|
||||||
|
{
|
||||||
|
if (angle_index >= 0 && angle_index < len) // 防止越界
|
||||||
|
{
|
||||||
|
this->upScan.distance[angle_index] = dis;
|
||||||
|
// cout << "dis: " << dis << " angle_index: " << angle_index << endl;
|
||||||
|
this->upScan.intensities[angle_index] = class_intensity[c + 1];
|
||||||
|
}
|
||||||
|
angle_index += 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
r->Z = Mat::zeros(2, 1, CV_32FC1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::optimize_clusterlable(vector<int> &clusterlable, Mat &bboxs) // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇
|
||||||
|
{
|
||||||
|
for (int i = 0; i < bboxs.rows; i++) // 寻找每个框对应的所有雷达簇
|
||||||
|
{
|
||||||
|
vector<int> temp_cluster_num;
|
||||||
|
Mat temp_bbox = bboxs.row(i).clone();
|
||||||
|
for (int j = 0; j < clusterlable.size(); j++)
|
||||||
|
{
|
||||||
|
if (clusterlable[j] == i)
|
||||||
|
{
|
||||||
|
temp_cluster_num.push_back(j);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (temp_cluster_num.size() == 0)
|
||||||
|
continue;
|
||||||
|
choose_forceground(temp_cluster_num, temp_bbox, clusterlable); // 筛选镂空物体的前景
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::choose_forceground(vector<int> &cluster_num, Mat &bbox, vector<int> &clusterlable) // 筛选镂空物体的前景
|
||||||
|
{
|
||||||
|
int box_class = bbox.at<float>(0, 5);
|
||||||
|
if ((class_index[box_class] >= 15 && class_index[box_class] <= 18 || class_index[box_class] == 12 || class_index[box_class] == 10) && cluster_num.size() > 1)
|
||||||
|
{
|
||||||
|
cout << "class " << box_class << " have " << cluster_num.size() << " clusters" << endl;
|
||||||
|
int total_point = 0;
|
||||||
|
for (int i = 0; i < cluster_num.size(); i++) // 计算框内雷达点总数
|
||||||
|
{
|
||||||
|
total_point += (this->upScan.clustedCloud[cluster_num[i]].size());
|
||||||
|
}
|
||||||
|
for (int j = 0; j < cluster_num.size(); j++) // 计算每个雷达簇占比，大于0.2认为是背景
|
||||||
|
{
|
||||||
|
float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size();
|
||||||
|
if ((num_cluster / total_point > 0.2) || num_cluster > 4)
|
||||||
|
{
|
||||||
|
clusterlable[cluster_num[j]] = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::draw_point(std::vector<Mat> &camInfo) // 画出映射到图像的雷达点
|
||||||
|
{
|
||||||
|
projectedPoints.clear();
|
||||||
|
for (int i = 0; i < this->upScan.pointCloud.rows; i++)
|
||||||
|
{
|
||||||
|
Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone();
|
||||||
|
Mat uv = pointcloud_pixel(temp_pointcloud);
|
||||||
|
int x = uv.at<float>(0, 0);
|
||||||
|
int y = uv.at<float>(1, 0);
|
||||||
|
if (x > 0 && x < 640 && y > 0 && y < 480)
|
||||||
|
{
|
||||||
|
Point2d pt_uv(x, y);
|
||||||
|
projectedPoints.push_back(pt_uv);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc)
|
||||||
|
// {
|
||||||
|
// upScan.distance.clear();
|
||||||
|
// upScan.intensities.clear();
|
||||||
|
// float pointAngle;
|
||||||
|
// upScan.angleInc = angleInc;
|
||||||
|
|
||||||
|
// for (int i = 0; i < laserData.len; i++)
|
||||||
|
// {
|
||||||
|
// pointAngle = angle_min + i * upScan.angleInc;
|
||||||
|
// if (pointAngle > cameraAnglemin)
|
||||||
|
// {
|
||||||
|
// if (pointAngle > cameraAnglemax)
|
||||||
|
// {
|
||||||
|
// upScan.angleMax = pointAngle - upScan.angleInc;
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// upScan.distance.push_back(laserData.distance[i] / 1000.f);
|
||||||
|
// upScan.intensities.push_back(0);
|
||||||
|
// if (i == (laserData.len - 1))
|
||||||
|
// upScan.angleMax = pointAngle;
|
||||||
|
// // upScan.intensities.push_back(laserData.intensities[i]);
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc;
|
||||||
|
// this->scan_to_pointcloud();
|
||||||
|
// this->cluster();
|
||||||
|
// }
|
||||||
|
void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) // 雷达点和相机视野同步
|
||||||
|
{
|
||||||
|
upScan.distance.clear();
|
||||||
|
upScan.intensities.clear();
|
||||||
|
upScan.angleMin = angle_min;
|
||||||
|
upScan.angleInc = angleInc;
|
||||||
|
upScan.angleMax = ANGLE_TO_RADIAN(laserData.angle_max);
|
||||||
|
float pointAngle;
|
||||||
|
for (int i = 0; i < laserData.len; i++)
|
||||||
|
{
|
||||||
|
upScan.distance.push_back(laserData.distance[i] / 1000.f);
|
||||||
|
pointAngle = angle_min + i * upScan.angleInc;
|
||||||
|
if (pointAngle > cameraAnglemin && pointAngle < cameraAnglemax)
|
||||||
|
upScan.intensities.push_back(1); // 视野范围内强度赋1
|
||||||
|
else
|
||||||
|
upScan.intensities.push_back(0); // 视野范围外强度赋0
|
||||||
|
}
|
||||||
|
|
||||||
|
this->scan_to_pointcloud(); // 雷达极坐标到二维坐标转换
|
||||||
|
this->cluster(); // 雷达聚类
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::scan_to_pointcloud() // 雷达极坐标到二维坐标转换
|
||||||
|
{
|
||||||
|
u_int32_t len = upScan.distance.size();
|
||||||
|
Mat pointcloud(len, 4, CV_32F);
|
||||||
|
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
float dis = this->upScan.distance[i];
|
||||||
|
float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis;
|
||||||
|
float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis;
|
||||||
|
pointcloud.at<float>(i, 0) = x_temp;
|
||||||
|
pointcloud.at<float>(i, 1) = y_temp;
|
||||||
|
pointcloud.at<float>(i, 2) = 0;
|
||||||
|
pointcloud.at<float>(i, 3) = i;
|
||||||
|
}
|
||||||
|
this->upScan.pointCloud = pointcloud.clone();
|
||||||
|
}
|
||||||
|
|
||||||
|
uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag) // 无用
|
||||||
|
{
|
||||||
|
int x = uv.at<float>(0, 0);
|
||||||
|
int y = uv.at<float>(1, 0);
|
||||||
|
// 实验测试，过滤类别
|
||||||
|
std::set<uchar> filterCls = {0, 7, 11, 14, 15, 21};
|
||||||
|
for (uchar i = 0; i < boxes.rows; i++)
|
||||||
|
{
|
||||||
|
int clsIdx = boxes.at<float>(i, 5);
|
||||||
|
if (filterCls.find(clsIdx) != filterCls.end())
|
||||||
|
continue;
|
||||||
|
|
||||||
|
float xmin = boxes.at<float>(i, 0);
|
||||||
|
float xmax = boxes.at<float>(i, 2);
|
||||||
|
float ymin = boxes.at<float>(i, 1);
|
||||||
|
float ymax = boxes.at<float>(i, 3);
|
||||||
|
|
||||||
|
// 实验测试，过滤过大的误检框
|
||||||
|
float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
|
||||||
|
if (ratio > 0.7)
|
||||||
|
{
|
||||||
|
boxflag[i] += 1;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况
|
||||||
|
if (x >= xmin && x <= xmax && y >= ymin && y <= ymax)
|
||||||
|
{
|
||||||
|
boxflag[i] += 1;
|
||||||
|
return class_intensity[clsIdx + 1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<int> LCFusion::clusters_bboxs(Mat &bboxs) // 所有的聚类的雷达簇和所有的检测框的匹配
|
||||||
|
{
|
||||||
|
std::vector<int> clusterlable;
|
||||||
|
for (int i = 0; i < this->upScan.clustedCloud.size(); i++) // 遍历每簇雷达
|
||||||
|
{
|
||||||
|
std::vector<Mat> temp_cluster = this->upScan.clustedCloud[i];
|
||||||
|
int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs); // 为一簇雷达寻找匹配的检测框
|
||||||
|
clusterlable.push_back(temp_clusters_bbox);
|
||||||
|
}
|
||||||
|
return clusterlable;
|
||||||
|
}
|
||||||
|
|
||||||
|
int LCFusion::cluster_bboxs(vector<Mat> &one_cluster, Mat &bboxs) // 为一簇雷达寻找匹配的检测框
|
||||||
|
{
|
||||||
|
vector<int> temp_result;
|
||||||
|
Mat begin = pointcloud_pixel(one_cluster[one_cluster.size() - 1]);
|
||||||
|
Mat end = pointcloud_pixel(one_cluster[0]); // 一簇雷达起始点和终止点在像素坐标系的坐标
|
||||||
|
for (int i = 0; i < bboxs.rows; i++)
|
||||||
|
{
|
||||||
|
Mat bbox = bboxs.row(i).clone();
|
||||||
|
int cls = bbox.at<float>(0, 5);
|
||||||
|
float ratio = ratio_in_1box(begin, end, bbox); // 在框中占比
|
||||||
|
if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18) // 适用于大物体
|
||||||
|
temp_result.push_back(i);
|
||||||
|
}
|
||||||
|
if (temp_result.size() < 1) // 若一簇雷达落入多个框里，选择宽度较小的框
|
||||||
|
return -1;
|
||||||
|
else if (temp_result.size() < 2)
|
||||||
|
return temp_result[0];
|
||||||
|
else
|
||||||
|
{
|
||||||
|
int min = temp_result[0];
|
||||||
|
int width_min = bboxs.at<float>(min, 2) - bboxs.at<float>(min, 0);
|
||||||
|
for (int i = 0; i < temp_result.size(); i++)
|
||||||
|
{
|
||||||
|
int width_new = bboxs.at<float>(temp_result[i], 2) - bboxs.at<float>(temp_result[i], 0);
|
||||||
|
if (width_new < width_min)
|
||||||
|
{
|
||||||
|
min = temp_result[i];
|
||||||
|
width_min = width_new;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return min;
|
||||||
|
}
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
Mat LCFusion::pointcloud_pixel(Mat &pointcloud) // 二维雷达点到像素坐标系转换
|
||||||
|
{
|
||||||
|
Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3));
|
||||||
|
Mat camPoint = rotate * cutPointCloud.t() + translation;
|
||||||
|
// float ppp = camPoint.at<float>(2, 0);
|
||||||
|
if (camPoint.at<float>(2, 0) <= 0)
|
||||||
|
{
|
||||||
|
uv = (Mat_<float>(3, 1) << -1, -1, -1);
|
||||||
|
return uv;
|
||||||
|
}
|
||||||
|
|
||||||
|
float scaleX = camPoint.at<float>(0, 0) / camPoint.at<float>(2, 0);
|
||||||
|
float scaleY = camPoint.at<float>(1, 0) / camPoint.at<float>(2, 0);
|
||||||
|
float scaleD = scaleX * scaleX + scaleY * scaleY;
|
||||||
|
float tempD = 1 + distCoeff.at<float>(0, 0) * scaleD + distCoeff.at<float>(0, 1) * scaleD * scaleD + distCoeff.at<float>(0, 4) * scaleD * scaleD * scaleD;
|
||||||
|
|
||||||
|
camPoint.at<float>(0, 0) = scaleX * tempD + 2 * distCoeff.at<float>(0, 2) * scaleX * scaleY +
|
||||||
|
distCoeff.at<float>(0, 3) * (scaleD + 2 * scaleX * scaleX);
|
||||||
|
|
||||||
|
camPoint.at<float>(1, 0) = scaleY * tempD + distCoeff.at<float>(0, 2) * (scaleD + 2 * scaleY * scaleY) +
|
||||||
|
2 * distCoeff.at<float>(0, 3) * scaleX * scaleY;
|
||||||
|
|
||||||
|
camPoint.at<float>(2, 0) = 1.0;
|
||||||
|
uv = cameraMat * camPoint; // uv为像素坐标
|
||||||
|
uv = p2r * uv + t; // 矫正
|
||||||
|
return uv;
|
||||||
|
}
|
||||||
|
|
||||||
|
float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box) // 计算一簇雷达落入检测框的比例
|
||||||
|
{
|
||||||
|
int x_begin = begin.at<float>(0, 0), x_end = end.at<float>(0, 0), y_begin = begin.at<float>(1, 0), y_end = end.at<float>(1, 0);
|
||||||
|
int xmin = box.at<float>(0, 0), ymin = box.at<float>(0, 1), xmax = box.at<float>(0, 2), ymax = box.at<float>(0, 3);
|
||||||
|
if (y_begin < ymin || y_begin > ymax)
|
||||||
|
return 0;
|
||||||
|
if (x_begin < xmin)
|
||||||
|
{
|
||||||
|
if (x_end < xmin)
|
||||||
|
return 0;
|
||||||
|
else if (x_end < xmax)
|
||||||
|
return (float)(x_end - xmin) / (x_end - x_begin);
|
||||||
|
else
|
||||||
|
return (float)(xmax - xmin) / (x_end - x_begin);
|
||||||
|
}
|
||||||
|
else if (x_begin < xmax)
|
||||||
|
{
|
||||||
|
if (x_end < xmax)
|
||||||
|
return 1;
|
||||||
|
else
|
||||||
|
return (float)(xmax - x_begin) / (x_end - x_begin);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::cluster() // 聚类
|
||||||
|
{
|
||||||
|
this->upScan.clustedCloud.clear();
|
||||||
|
int i = 0;
|
||||||
|
|
||||||
|
for (; i < this->upScan.distance.size();)
|
||||||
|
{
|
||||||
|
std::vector<Mat> temp;
|
||||||
|
int j = i;
|
||||||
|
temp.push_back(this->upScan.pointCloud.row(i));
|
||||||
|
for (; j < this->upScan.distance.size() - 1; j++)
|
||||||
|
{
|
||||||
|
|
||||||
|
// 距离的6%作为分类的阈值
|
||||||
|
if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 )
|
||||||
|
{
|
||||||
|
temp.push_back(this->upScan.pointCloud.row(j + 1));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
i = j + 1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
this->upScan.clustedCloud.push_back(temp);
|
||||||
|
if (j == this->upScan.distance.size() - 1)
|
||||||
|
break;
|
||||||
|
if (i == upScan.distance.size() - 1)
|
||||||
|
{
|
||||||
|
temp.push_back(upScan.pointCloud.row(i));
|
||||||
|
this->upScan.clustedCloud.push_back(temp);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
this->clusterIdx.clear();
|
||||||
|
int ii = 0, totoalNum = 0;
|
||||||
|
for (auto c : this->upScan.clustedCloud)
|
||||||
|
{
|
||||||
|
for (int jj = 0; jj < c.size(); jj++)
|
||||||
|
{
|
||||||
|
this->clusterIdx.push_back(ii);
|
||||||
|
}
|
||||||
|
totoalNum += c.size();
|
||||||
|
ii++;
|
||||||
|
}
|
||||||
|
//std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl;
|
||||||
|
}
|
||||||
|
void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha) // 扩散物体生成圆弧状伪激光雷达
|
||||||
|
{
|
||||||
|
vector<float> center_point = find_circle_center(cluster_num);
|
||||||
|
float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at<float>(bbox_num, 1)/200.f,
|
||||||
|
d = center_point[0], half_data_num = atan(r/d)/(upScan.angleInc);
|
||||||
|
int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at<float>(bbox_num, 5);
|
||||||
|
float laser_index = center_point[1] - half_data_num;
|
||||||
|
//cout << "alfa: " << alfa << " r: " << r << " d: " << d << " num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl;
|
||||||
|
for (int i = 0; i < num_laser_circle; i++)
|
||||||
|
{
|
||||||
|
float theta = upScan.angleMin + laser_index * upScan.angleInc;
|
||||||
|
if (laser_index < 0 || laser_index > upScan.distance.size())
|
||||||
|
{
|
||||||
|
laser_index++;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2);
|
||||||
|
if (data >= 0)
|
||||||
|
{
|
||||||
|
//cout << "draw_one_point" << endl;
|
||||||
|
upScan.distance[laser_index] = circle(d, alfa ,theta, data);
|
||||||
|
upScan.intensities[laser_index] = class_intensity[cls + 1];
|
||||||
|
}
|
||||||
|
laser_index++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
vector<float> LCFusion::find_circle_center(int cluster_num) // 寻找扩散物体圆弧的圆心
|
||||||
|
{
|
||||||
|
vector<float> center_point(2);
|
||||||
|
int index = 0;
|
||||||
|
float total_dis = 0;
|
||||||
|
int num = upScan.clustedCloud[cluster_num].size();
|
||||||
|
int no_zero_dis = 0; // 非零距离和
|
||||||
|
for (int i = 0 ; i < num; i++)
|
||||||
|
{
|
||||||
|
index = upScan.clustedCloud[cluster_num][i].at<float>(0, 3);
|
||||||
|
if (upScan.distance[index] != 0)
|
||||||
|
no_zero_dis++;
|
||||||
|
total_dis += upScan.distance[index];
|
||||||
|
}
|
||||||
|
center_point[0] = total_dis/ no_zero_dis;
|
||||||
|
index -= (num/2);
|
||||||
|
float center_laser_index = index;
|
||||||
|
center_point[1] = center_laser_index;
|
||||||
|
return center_point;
|
||||||
|
}
|
||||||
|
float LCFusion::circle(float d, float alfa ,float theta, float data) // d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度
|
||||||
|
{
|
||||||
|
float ro = d * cos(theta - alfa) - sqrt(data);
|
||||||
|
return ro > 0 ? ro:0;
|
||||||
|
}
|
||||||
|
vector<float> LCFusion::width_ladar(const int* coordinates)
|
||||||
|
{
|
||||||
|
int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
|
||||||
|
|
||||||
|
std::vector<float> mid = r-> pic2cam(640 / 2, 480); //<2F>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
std::vector<float> loc_tar_start = r-> pic2cam(x1, 480); // <20>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC><CEBB>
|
||||||
|
std::vector<float> loc_tar_end = r-> pic2cam(x2, 480); //<2F>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ֹλ<D6B9><CEBB>
|
||||||
|
//ת<><D7AA><EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD>ֵ
|
||||||
|
float alfa_start = atan((loc_tar_start[0] - mid[0]) / r-> q.at<double>(2, 3));
|
||||||
|
float alfa_end = atan((loc_tar_end[0] - mid[0]) / r-> q.at<double>(2, 3));
|
||||||
|
std::vector<float> ladar_range;
|
||||||
|
ladar_range.push_back(alfa_start);
|
||||||
|
ladar_range.push_back(alfa_end);
|
||||||
|
return ladar_range;
|
||||||
|
}
|
||||||
|
void LCFusion::small_box_without_dis(Mat & bboxs, Mat & dwha) // 无距离检测框到像素坐标系映射，过程和small_fusion类似
|
||||||
|
{
|
||||||
|
std::cout << "small_box_without_dis" << std::endl;
|
||||||
|
double set_dis = 2.0; // 设置距离为2米
|
||||||
|
for (int i = 0; i < bboxs.rows; i++)
|
||||||
|
{
|
||||||
|
int c = bboxs.at<float>(i, 5);
|
||||||
|
int x_mim = bboxs.at<float>(i, 0);
|
||||||
|
int y_min = bboxs.at<float>(i, 1);
|
||||||
|
int x_max = bboxs.at<float>(i, 2);
|
||||||
|
int y_max = bboxs.at<float>(i, 3);
|
||||||
|
double d = dwha.at<float>(i, 0);
|
||||||
|
int coordinates[4] = {x_mim, y_min, x_max, y_max};
|
||||||
|
if ((class_index[c] >= 0 && class_index[c] <= 1) || (class_index[c] >= 3 && class_index[c] <= 6))
|
||||||
|
{
|
||||||
|
|
||||||
|
Mat temp(2, 1, CV_32F);
|
||||||
|
|
||||||
|
|
||||||
|
if (d == -1)
|
||||||
|
{
|
||||||
|
vector<float> box_angle = width_ladar(coordinates);
|
||||||
|
//std::cout << "d:" << d << " a: " << a << std::endl;
|
||||||
|
temp.at<float>(0, 0) = float(set_dis * cos(box_angle[0]));
|
||||||
|
temp.at<float>(1, 0) = float(set_dis * sin(box_angle[0]));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
double y = temp.at<float>(0, 0);
|
||||||
|
double x = temp.at<float>(1, 0);
|
||||||
|
float dis = sqrt(x * x + y * y);
|
||||||
|
float angle_end = 0.0;
|
||||||
|
if (y >= 0)
|
||||||
|
angle_end = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle_end = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
|
||||||
|
angle_end = int(angle_end);
|
||||||
|
// std:: cout << "angle end: " << angle_end << std::endl;
|
||||||
|
temp.at<float>(0, 0) = float(set_dis * cos(box_angle[1]));
|
||||||
|
temp.at<float>(1, 0) = float(set_dis * sin(box_angle[1]));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
y = temp.at<float>(0, 0);
|
||||||
|
x = temp.at<float>(1, 0);
|
||||||
|
dis = sqrt(x * x + y * y);
|
||||||
|
float angle_start = 0.0;
|
||||||
|
if (y >= 0)
|
||||||
|
angle_start = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle_start = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
|
||||||
|
angle_start = int(angle_start);
|
||||||
|
// std:: cout << "angle start: " << angle_start << std::endl;
|
||||||
|
if (angle_end-angle_start <= 0)
|
||||||
|
continue;
|
||||||
|
int num_point = angle_end-angle_start+1;
|
||||||
|
for (int i = 0; i < num_point; i++)
|
||||||
|
{
|
||||||
|
if ((angle_start + i) < 0 || (angle_start + i) > (upScan.distance.size() - 1))
|
||||||
|
continue;
|
||||||
|
this->upScan.intensities[angle_start + i] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData) // 设置雷达发布数据（原始雷达）
|
||||||
|
{
|
||||||
|
float angle_min = ANGLE_TO_RADIAN(laserData.angle_min);
|
||||||
|
float angle_max = ANGLE_TO_RADIAN(laserData.angle_max);
|
||||||
|
uint32_t len = laserData.len;
|
||||||
|
float angle_inc = (angle_max - angle_min) / (len - 1.);
|
||||||
|
scan.header.frame_id = "down_laser_link";
|
||||||
|
scan.range_min = 0.15;
|
||||||
|
scan.range_max = 8.0;
|
||||||
|
scan.angle_min = angle_min;
|
||||||
|
scan.angle_max = angle_max;
|
||||||
|
scan.angle_increment = angle_inc;
|
||||||
|
scan.ranges.resize(len);
|
||||||
|
scan.intensities.resize(len);
|
||||||
|
scan.time_increment = 1. / 2400.;
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
scan.ranges[i] = laserData.distance[i] / 1000.f;
|
||||||
|
scan.intensities[i] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan) // 设置雷达发布数据（语义雷达）
|
||||||
|
{
|
||||||
|
scan.header.frame_id = "up_laser_link";
|
||||||
|
scan.range_min = 0.15;
|
||||||
|
scan.range_max = 8.0;
|
||||||
|
scan.angle_increment = upScan.angleInc;
|
||||||
|
scan.ranges.resize(upScan.distance.size());
|
||||||
|
scan.intensities.resize(upScan.distance.size());
|
||||||
|
scan.angle_min = upScan.angleMin;
|
||||||
|
scan.angle_max = upScan.angleMax;
|
||||||
|
for (int i = 0; i < upScan.distance.size(); i++)
|
||||||
|
{
|
||||||
|
scan.ranges[i] = upScan.distance[i];
|
||||||
|
scan.intensities[i] = upScan.intensities[i];
|
||||||
|
}
|
||||||
|
scan.time_increment = 1. / 2400.;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
100
fusion.h
Normal file
100
fusion.h
Normal file
@ -0,0 +1,100 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/LaserScan.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "lidar.h"
|
||||||
|
#include "ranging.h"
|
||||||
|
|
||||||
|
using namespace cv;
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
struct ImgScan
|
||||||
|
{
|
||||||
|
float angleMin; //弧度
|
||||||
|
float angleMax;
|
||||||
|
float angleInc;
|
||||||
|
std::vector<float> distance; // 米
|
||||||
|
std::vector<uint8_t> intensities;
|
||||||
|
Mat pointCloud;
|
||||||
|
std::vector<std::vector<Mat>> clustedCloud;
|
||||||
|
};
|
||||||
|
class Ranging; //友元类
|
||||||
|
class LCFusion
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
LCFusion(Ranging* ranging)
|
||||||
|
{
|
||||||
|
r = ranging;
|
||||||
|
rotate = cameraExtrinsicMat.rowRange(0, 3).colRange(0, 3).t(); // 雷达 -> 相机坐标系旋转矩阵
|
||||||
|
translation = cameraExtrinsicMat.col(3).rowRange(0, 3);
|
||||||
|
translation = -rotate * translation; // 雷达 -> 相机坐标系平移矩阵
|
||||||
|
p2r = r_y * r_c; // 像素坐标系矫正矩阵（旋转）
|
||||||
|
};
|
||||||
|
char fusion(std::vector<Mat> &camInfo); // fusion函数
|
||||||
|
uchar lidar2box(Mat uv, Mat boxes, uchar *boxFlag); // 无用
|
||||||
|
void filter_laser(const FrameData &lidarInfo, float angle_min, float angle_inc); // 雷达点和相机视野同步
|
||||||
|
void scan_to_pointcloud(); // 雷达极坐标到二维坐标转换
|
||||||
|
std::vector<int> clusters_bboxs(Mat &bboxs); // 所有的聚类的雷达簇和所有的检测框的匹配
|
||||||
|
int cluster_bboxs(std::vector<Mat> &one_cluster, Mat &bboxs); // 寻找一簇雷达簇匹配的检测框
|
||||||
|
void optimize_clusterlable(vector<int> & clusterlable, Mat & bboxs); // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇
|
||||||
|
void choose_forceground(vector<int> & cluster_num, Mat & bbox, vector<int> & clusterlable); // 针对镂空物体选择对应的雷达簇，筛选过程
|
||||||
|
void big_fusion(vector<int> & clusterlable, Mat & bboxs, Mat &dwha); // 大物体赋语义
|
||||||
|
void small_fusion(Mat & bboxs, Mat & dwha); // 小物体赋语义
|
||||||
|
Mat pointcloud_pixel(Mat &pointcloud); // 二维雷达点到像素坐标系转换
|
||||||
|
void cluster(); // 聚类
|
||||||
|
float ratio_in_1box(Mat &begin, Mat &end, Mat &box); // 计算一簇雷达落入检测框的比例
|
||||||
|
void draw_point(std::vector<Mat> &camInfo); // 画出雷达在图像中的映射点
|
||||||
|
void draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha); // 扩散物体生成圆弧状伪激光雷达
|
||||||
|
vector<float> find_circle_center(int cluster_num); // 寻找扩散物体圆弧的圆心
|
||||||
|
float circle(float d, float alfa ,float theta, float data); // 圆弧角度和距离的数学关系
|
||||||
|
vector<float> width_ladar(const int* coordinates);
|
||||||
|
void small_box_without_dis(Mat & bboxs, Mat & dwha); // 检测框到雷达坐标系的映射关系
|
||||||
|
void set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData); // 设置雷达发布数据（原始雷达）
|
||||||
|
void set_laser_data(sensor_msgs::LaserScan& scan); // 设置雷达发布数据（语义雷达）
|
||||||
|
|
||||||
|
ImgScan upScan;
|
||||||
|
// 调试使用，将雷达点映射到图像中显示
|
||||||
|
vector<Point2f> projectedPoints; // 雷达 -> 像素坐标系映射的点
|
||||||
|
std::vector<uchar> clusterIdx;
|
||||||
|
|
||||||
|
private:
|
||||||
|
Ranging* r;
|
||||||
|
//float cameraAnglemin = 1.117, cameraAnglemax = 2.0071;
|
||||||
|
float cameraAnglemin = 1.345, cameraAnglemax = 2.0595; // 相机视野角度
|
||||||
|
Mat rotate, translation, p2r; // 雷达点映射矫正
|
||||||
|
Mat cameraExtrinsicMat = (Mat_<float>(4, 4) << 9.9710325100937192e-01, 5.9677185357037893e-02,
|
||||||
|
-4.7156551765403301e-02, 2.6591864512557874e-02,
|
||||||
|
4.7136795335845721e-02, 1.7395474156941537e-03,
|
||||||
|
9.9888692878636431e-01, 7.4034983851991365e-02,
|
||||||
|
5.9692791457662736e-02, -9.9821621281296091e-01,
|
||||||
|
-1.0784828889205400e-03, -9.6176066499866750e-02, 0., 0., 0., 1.);
|
||||||
|
|
||||||
|
Mat cameraMat = (Mat_<float>(3, 3) << 5.0280958247840368e+02, 0., 3.1925413622163182e+02, 0.,
|
||||||
|
5.0206199606708202e+02, 2.1999125419411467e+02, 0., 0., 1.);
|
||||||
|
|
||||||
|
Mat distCoeff = (Mat_<float>(1, 5) << 1.2903666948900971e-01, -9.2734018203156590e-02,
|
||||||
|
-7.0528508350750380e-03, 8.3183285124261049e-04,
|
||||||
|
7.2727934388070986e-02);
|
||||||
|
|
||||||
|
// Mat c2lR = (Mat_<float>(2, 2) << 0.99664806, 0.0802792, -0.0802792, 0.99664806);
|
||||||
|
Mat c2lR = (Mat_<float>(2, 2) << 0.98163, 0.19081, -0.19081, 0.98163); //11
|
||||||
|
//Mat c2lR = (Mat_<float>(2, 2) << 0.9703, 0.24192, -0.24192, 0.9703); //13
|
||||||
|
Mat c2lT = (Mat_<float>(2, 1) << 0.15, 0.01);
|
||||||
|
//Mat c2lT = (Mat_<float>(2, 1) << 0.13693697, 0.05094143);
|
||||||
|
Mat r_c = (Mat_<float>(3, 3) << 9.977e-1, 6.7744e-02, 0., -6.7744e-02, 9.977e-1, 0., 0., 0., 1.);
|
||||||
|
Mat t = (Mat_<float>(3, 1) << 50., 0., 0.);
|
||||||
|
Mat r_y = (Mat_<float>(3, 3) << 1., 0., 0., 0., 1.19444e+00, 0., 0., 0., 1.);
|
||||||
|
/*int class_intensity[24] = {
|
||||||
|
0, 120, 130, 130, 130, 130, 130, 130, 146, 146,
|
||||||
|
146, 162, 162, 178, 178, 196, 196, 212, 212, 228,
|
||||||
|
228, 0, 0, 0}; */
|
||||||
|
int class_intensity[25] = {
|
||||||
|
0, 115, 116, 117, 118, 119, 120, 121, 150, 151,
|
||||||
|
152, 153, 154, 155, 156, 200, 201, 202, 203, 204,
|
||||||
|
205, 206, 207, 208, 209}; // 物体类别和强度映射关系
|
||||||
|
|
||||||
|
map<int, int> class_index = {{-1, 23}, {0, 0}, {1, 13}, {2, 14}, {3, 7}, {4, 8}, {5, 9}, {6, 1},
|
||||||
|
{7, 2}, {8, 19}, {9, 18}, {10, 6}, {11, 3}, {12, 15}, {13, 10}, {14, 4}, {15, 5}, {16, 11}, {17, 16},
|
||||||
|
{18, 12}, {19, 17}, {20, 20}, {21, 21}, {22, 22}};
|
||||||
|
}; // 类别映射，方便大小物体判断
|
||||||
583
lidar.cpp
Normal file
583
lidar.cpp
Normal file
@ -0,0 +1,583 @@
|
|||||||
|
/**
|
||||||
|
* @file lipkg.cpp
|
||||||
|
* @author LD Robot
|
||||||
|
* @version V01
|
||||||
|
* @brief
|
||||||
|
* @note
|
||||||
|
* @attention COPYRIGHT LDROBOT
|
||||||
|
**/
|
||||||
|
|
||||||
|
#include "lidar.h"
|
||||||
|
#include <math.h>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
// #ifdef USE_SLBI
|
||||||
|
// #include "slbi.h"
|
||||||
|
// #endif
|
||||||
|
|
||||||
|
// #ifdef USE_SLBF
|
||||||
|
// #include "slbf.h"
|
||||||
|
// #endif
|
||||||
|
//#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59/180000)
|
||||||
|
#define MAX_ACK_BUF_LEN 2304000
|
||||||
|
|
||||||
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
||||||
|
|
||||||
|
static const uint8_t CrcTable[256] =
|
||||||
|
{
|
||||||
|
0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3,
|
||||||
|
0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33,
|
||||||
|
0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8,
|
||||||
|
0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77,
|
||||||
|
0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
|
||||||
|
0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4,
|
||||||
|
0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f,
|
||||||
|
0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff,
|
||||||
|
0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2,
|
||||||
|
0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12,
|
||||||
|
0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99,
|
||||||
|
0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14,
|
||||||
|
0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36,
|
||||||
|
0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9,
|
||||||
|
0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72,
|
||||||
|
0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2,
|
||||||
|
0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1,
|
||||||
|
0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71,
|
||||||
|
0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa,
|
||||||
|
0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35,
|
||||||
|
0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17,
|
||||||
|
0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8};
|
||||||
|
|
||||||
|
CmdInterfaceLinux::CmdInterfaceLinux(int32_t ver) : mRxThread(nullptr),
|
||||||
|
mRxCount(0),
|
||||||
|
mReadCallback(nullptr),
|
||||||
|
version(ver)
|
||||||
|
{
|
||||||
|
mComHandle = -1;
|
||||||
|
}
|
||||||
|
CmdInterfaceLinux::~CmdInterfaceLinux()
|
||||||
|
{
|
||||||
|
Close();
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::Open(std::string &port_name)
|
||||||
|
{
|
||||||
|
int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||||
|
|
||||||
|
mComHandle = open(port_name.c_str(), flags);
|
||||||
|
if (-1 == mComHandle)
|
||||||
|
{
|
||||||
|
std::cout << "CmdInterfaceLinux::Open " << port_name << " error !" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get port options
|
||||||
|
struct termios options;
|
||||||
|
if (-1 == tcgetattr(mComHandle, &options))
|
||||||
|
{
|
||||||
|
Close();
|
||||||
|
std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS);
|
||||||
|
options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD);
|
||||||
|
options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL |
|
||||||
|
ISIG | IEXTEN); //|ECHOPRT
|
||||||
|
options.c_oflag &= (tcflag_t) ~(OPOST);
|
||||||
|
options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK);
|
||||||
|
|
||||||
|
options.c_cc[VMIN] = 0;
|
||||||
|
options.c_cc[VTIME] = 0;
|
||||||
|
|
||||||
|
switch (version)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
case 6:
|
||||||
|
cfsetispeed(&options, B230400);
|
||||||
|
break;
|
||||||
|
case 8:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
case 14:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (tcsetattr(mComHandle, TCSANOW, &options) < 0)
|
||||||
|
{
|
||||||
|
std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl;
|
||||||
|
Close();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
tcflush(mComHandle, TCIFLUSH);
|
||||||
|
|
||||||
|
mRxThreadExitFlag = false;
|
||||||
|
mRxThread = new std::thread(mRxThreadProc, this);
|
||||||
|
mIsCmdOpened = true;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::Close()
|
||||||
|
{
|
||||||
|
if (mIsCmdOpened == false)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
mRxThreadExitFlag = true;
|
||||||
|
|
||||||
|
if (mComHandle != -1)
|
||||||
|
{
|
||||||
|
close(mComHandle);
|
||||||
|
mComHandle = -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((mRxThread != nullptr) && mRxThread->joinable())
|
||||||
|
{
|
||||||
|
mRxThread->join();
|
||||||
|
delete mRxThread;
|
||||||
|
mRxThread = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
mIsCmdOpened = false;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::GetCmdDevices(std::vector<std::pair<std::string, std::string>> &device_list)
|
||||||
|
{
|
||||||
|
struct udev *udev;
|
||||||
|
struct udev_enumerate *enumerate;
|
||||||
|
struct udev_list_entry *devices, *dev_list_entry;
|
||||||
|
struct udev_device *dev;
|
||||||
|
|
||||||
|
udev = udev_new();
|
||||||
|
if (!udev)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
enumerate = udev_enumerate_new(udev);
|
||||||
|
udev_enumerate_add_match_subsystem(enumerate, "tty");
|
||||||
|
udev_enumerate_scan_devices(enumerate);
|
||||||
|
devices = udev_enumerate_get_list_entry(enumerate);
|
||||||
|
udev_list_entry_foreach(dev_list_entry, devices)
|
||||||
|
{
|
||||||
|
const char *path;
|
||||||
|
|
||||||
|
path = udev_list_entry_get_name(dev_list_entry);
|
||||||
|
dev = udev_device_new_from_syspath(udev, path);
|
||||||
|
std::string dev_path = std::string(udev_device_get_devnode(dev));
|
||||||
|
dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device");
|
||||||
|
if (dev)
|
||||||
|
{
|
||||||
|
std::pair<std::string, std::string> p;
|
||||||
|
p.first = dev_path;
|
||||||
|
p.second = udev_device_get_sysattr_value(dev, "product");
|
||||||
|
device_list.push_back(p);
|
||||||
|
udev_device_unref(dev);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
udev_enumerate_unref(enumerate);
|
||||||
|
udev_unref(udev);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len)
|
||||||
|
{
|
||||||
|
static timespec timeout = {0, (long)(100 * 1e6)};
|
||||||
|
int32_t len = -1;
|
||||||
|
|
||||||
|
if (IsOpened())
|
||||||
|
{
|
||||||
|
fd_set read_fds;
|
||||||
|
FD_ZERO(&read_fds);
|
||||||
|
FD_SET(mComHandle, &read_fds);
|
||||||
|
int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL);
|
||||||
|
if (r < 0)
|
||||||
|
{
|
||||||
|
// Select was interrupted
|
||||||
|
if (errno == EINTR)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (r == 0) // timeout
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (FD_ISSET(mComHandle, &read_fds))
|
||||||
|
{
|
||||||
|
len = (int32_t)read(mComHandle, rx_buf, rx_buf_len);
|
||||||
|
if ((len != -1) && rx_len)
|
||||||
|
{
|
||||||
|
*rx_len = len;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return len == -1 ? false : true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len)
|
||||||
|
{
|
||||||
|
int32_t len = -1;
|
||||||
|
|
||||||
|
if (IsOpened())
|
||||||
|
{
|
||||||
|
len = (int32_t)write(mComHandle, tx_buf, tx_buf_len);
|
||||||
|
if ((len != -1) && tx_len)
|
||||||
|
{
|
||||||
|
*tx_len = len;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return len == -1 ? false : true;
|
||||||
|
}
|
||||||
|
void CmdInterfaceLinux::mRxThreadProc(void *param)
|
||||||
|
{
|
||||||
|
CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param;
|
||||||
|
char *rx_buf = new char[MAX_ACK_BUF_LEN + 1];
|
||||||
|
uchar failedNum = 0;
|
||||||
|
while (!cmd_if->mRxThreadExitFlag.load())
|
||||||
|
{
|
||||||
|
uint32_t readed = 0;
|
||||||
|
bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed);
|
||||||
|
if (res && readed)
|
||||||
|
{
|
||||||
|
cmd_if->mRxCount += readed;
|
||||||
|
if (cmd_if->mReadCallback != nullptr)
|
||||||
|
{
|
||||||
|
cmd_if->mReadCallback(rx_buf, readed);
|
||||||
|
}
|
||||||
|
failedNum = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if(++failedNum == 255)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cmd_if->mRxThreadExitFlag = true;
|
||||||
|
delete[] rx_buf;
|
||||||
|
}
|
||||||
|
|
||||||
|
LiPkg::LiPkg() : mTimestamp(0),
|
||||||
|
mSpeed(0),
|
||||||
|
mErrorTimes(0),
|
||||||
|
mIsFrameReady(false),
|
||||||
|
mIsPkgReady(false)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
double LiPkg::GetSpeed(void)
|
||||||
|
{
|
||||||
|
return mSpeed / 360.0;
|
||||||
|
}
|
||||||
|
bool LiPkg::Parse(const uint8_t *data, long len)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
mDataTmp.push_back(*(data + i));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100)
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
mDataTmp.clear();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t start = 0;
|
||||||
|
|
||||||
|
while (start < mDataTmp.size() - 2)
|
||||||
|
{
|
||||||
|
start = 0;
|
||||||
|
while (start < mDataTmp.size() - 2)
|
||||||
|
{
|
||||||
|
if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN))
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5))))
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
start++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (start != 0)
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
for (int i = 0; i < start; i++)
|
||||||
|
{
|
||||||
|
mDataTmp.erase(mDataTmp.begin());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
|
||||||
|
return false;
|
||||||
|
|
||||||
|
LiDARFrameTypeDef *pkg = (LiDARFrameTypeDef *)mDataTmp.data();
|
||||||
|
uint8_t crc = 0;
|
||||||
|
|
||||||
|
for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++)
|
||||||
|
{
|
||||||
|
crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (crc == pkg->crc8)
|
||||||
|
{
|
||||||
|
double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360;
|
||||||
|
if (diff > (double)pkg->speed * POINT_PER_PACK / 2300 * 3 / 2)
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mSpeed = pkg->speed;
|
||||||
|
mTimestamp = pkg->timestamp;
|
||||||
|
uint32_t diff = ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000;
|
||||||
|
float step = diff / (POINT_PER_PACK - 1) / 100.0;
|
||||||
|
float start = (double)pkg->start_angle / 100.0;
|
||||||
|
float end = (double)(pkg->end_angle % 36000) / 100.0;
|
||||||
|
PointData data;
|
||||||
|
for (int i = 0; i < POINT_PER_PACK; i++)
|
||||||
|
{
|
||||||
|
data.distance = pkg->point[i].distance;
|
||||||
|
data.angle = start + i * step;
|
||||||
|
if (data.angle >= 360.0)
|
||||||
|
{
|
||||||
|
data.angle -= 360.0;
|
||||||
|
}
|
||||||
|
data.confidence = pkg->point[i].confidence;
|
||||||
|
mOnePkg[i] = data;
|
||||||
|
mFrameTemp.push_back(PointData(data.angle, data.distance, data.confidence));
|
||||||
|
}
|
||||||
|
// prevent angle invert
|
||||||
|
mOnePkg.back().angle = end;
|
||||||
|
|
||||||
|
mIsPkgReady = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++)
|
||||||
|
{
|
||||||
|
mDataTmp.erase(mDataTmp.begin());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
/*only remove header,not all frame,because lidar data may contain head*/
|
||||||
|
for (int i = 0; i < 2; i++)
|
||||||
|
{
|
||||||
|
mDataTmp.erase(mDataTmp.begin());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool LiPkg::AssemblePacket()
|
||||||
|
{
|
||||||
|
float last_angle = 0;
|
||||||
|
std::vector<PointData> tmp;
|
||||||
|
int count = 0;
|
||||||
|
for (auto n : mFrameTemp)
|
||||||
|
{
|
||||||
|
/*wait for enough data, need enough data to show a circle*/
|
||||||
|
if (n.angle - last_angle < (-350.f)) /* enough data has been obtained */
|
||||||
|
{
|
||||||
|
mFrameData.len = tmp.size();
|
||||||
|
Transform(tmp); /*transform raw data to stantard data */
|
||||||
|
|
||||||
|
if (tmp.size() == 0)
|
||||||
|
{
|
||||||
|
mFrameTemp.clear();
|
||||||
|
mIsFrameReady = false;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef USE_SLBI
|
||||||
|
Slbi sb(mSpeed);
|
||||||
|
sb.FindBarcode(tmp);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_SLBF
|
||||||
|
Slbf sb(mSpeed);
|
||||||
|
tmp = sb.NearFilter(tmp);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b)
|
||||||
|
{ return a.angle < b.angle; });
|
||||||
|
mFrameData.angle_min = tmp[0].angle;
|
||||||
|
mFrameData.angle_max = tmp.back().angle;
|
||||||
|
mFrameData.distance.clear();
|
||||||
|
mFrameData.intensities.clear();
|
||||||
|
mFrameData.distance.resize(mFrameData.len);
|
||||||
|
mFrameData.intensities.resize(mFrameData.len);
|
||||||
|
float step = (mFrameData.angle_max - mFrameData.angle_min) / mFrameData.len;
|
||||||
|
float angle_acc = mFrameData.angle_min;
|
||||||
|
int tmp_count = 0;
|
||||||
|
/* interpolation method */
|
||||||
|
for (uint32_t i = 0; i < mFrameData.len; i++)
|
||||||
|
{
|
||||||
|
if (angle_acc >= tmp[tmp_count].angle)
|
||||||
|
{
|
||||||
|
mFrameData.distance[i] = tmp[tmp_count].distance;
|
||||||
|
mFrameData.intensities[i] = tmp[tmp_count].confidence;
|
||||||
|
tmp_count++;
|
||||||
|
if (tmp_count == tmp.size())
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mFrameData.distance[i] = 0;
|
||||||
|
mFrameData.intensities[i] = 0;
|
||||||
|
}
|
||||||
|
angle_acc += step;
|
||||||
|
}
|
||||||
|
std::vector<PointData> tmp2;
|
||||||
|
for (uint32_t i = count; i < mFrameTemp.size(); i++)
|
||||||
|
{
|
||||||
|
tmp2.push_back(mFrameTemp[i]);
|
||||||
|
}
|
||||||
|
mFrameTemp.clear();
|
||||||
|
mFrameTemp = tmp2;
|
||||||
|
mIsFrameReady = true;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
tmp.push_back(n); /* getting data */
|
||||||
|
}
|
||||||
|
|
||||||
|
count++;
|
||||||
|
last_angle = n.angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const std::array<PointData, POINT_PER_PACK> &LiPkg::GetPkgData(void)
|
||||||
|
{
|
||||||
|
mIsPkgReady = false;
|
||||||
|
return mOnePkg;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<cv::Mat> LiPkg::scan_to_pointcloud(FrameData mFrameData)
|
||||||
|
{
|
||||||
|
u_int32_t len = mFrameData.len;
|
||||||
|
float angle_min = ANGLE_TO_RADIAN(mFrameData.angle_min);
|
||||||
|
float angle_max = ANGLE_TO_RADIAN(mFrameData.angle_max);
|
||||||
|
cv::Mat pointcloud2(len, 3, CV_32F);
|
||||||
|
cv::Mat intensities(1, len, CV_32F);
|
||||||
|
cv::Mat pointdis(1, len, CV_32F);
|
||||||
|
float a_increment = (angle_max - angle_min) / (float)(len - 1);
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
float dis = mFrameData.distance[i] / 1000.f;
|
||||||
|
intensities.at<float>(0, i) = mFrameData.intensities[i];
|
||||||
|
pointdis.at<float>(0, i) = dis;
|
||||||
|
|
||||||
|
float x_temp = cos(angle_min + i * a_increment) * dis;
|
||||||
|
float y_temp = sin(angle_min + i * a_increment) * dis;
|
||||||
|
pointcloud2.at<float>(i, 0) = x_temp;
|
||||||
|
pointcloud2.at<float>(i, 1) = y_temp;
|
||||||
|
pointcloud2.at<float>(i, 2) = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
return std::vector<cv::Mat>{pointcloud2, intensities, pointdis};
|
||||||
|
}
|
||||||
|
|
||||||
|
/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/
|
||||||
|
|
||||||
|
SlTransform::SlTransform(LDVersion version, bool to_right_hand)
|
||||||
|
{
|
||||||
|
switch (version)
|
||||||
|
{
|
||||||
|
case LDVersion::LD_ZERO:
|
||||||
|
case LDVersion::LD_NINE:
|
||||||
|
offset_x = 8.1;
|
||||||
|
offset_y = -22.5156;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case LDVersion::LD_THREE:
|
||||||
|
case LDVersion::LD_EIGHT:
|
||||||
|
offset_x = 5.9;
|
||||||
|
offset_y = -20.14;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case LDVersion::LD_FOURTEENTH:
|
||||||
|
offset_x = 5.9;
|
||||||
|
offset_y = -20.14;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
Points2D SlTransform::Transform(const Points2D &data)
|
||||||
|
{
|
||||||
|
Points2D tmp2;
|
||||||
|
for (auto n : data)
|
||||||
|
{
|
||||||
|
/*Filter out invalid data*/
|
||||||
|
if (n.distance == 0)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
/*transfer the origin to the center of lidar circle*/
|
||||||
|
/*The default direction of radar rotation is clockwise*/
|
||||||
|
/*transfer to the right-hand coordinate system*/
|
||||||
|
float right_hand = (360.f - n.angle);
|
||||||
|
double x = n.distance + offset_x;
|
||||||
|
double y = n.distance * 0.11923 + offset_y;
|
||||||
|
double d = sqrt(x * x + y * y);
|
||||||
|
double shift = atan(y / x) * 180.f / 3.14159;
|
||||||
|
/*Choose whether to use the right-hand system according to the flag*/
|
||||||
|
double angle;
|
||||||
|
if (to_right_hand)
|
||||||
|
angle = right_hand + shift;
|
||||||
|
else
|
||||||
|
angle = n.angle - shift;
|
||||||
|
|
||||||
|
if (angle > 360)
|
||||||
|
{
|
||||||
|
angle -= 360;
|
||||||
|
}
|
||||||
|
if (angle < 0)
|
||||||
|
{
|
||||||
|
angle += 360;
|
||||||
|
}
|
||||||
|
|
||||||
|
tmp2.push_back(PointData(angle, n.distance, n.confidence, x, y));
|
||||||
|
}
|
||||||
|
|
||||||
|
return tmp2;
|
||||||
|
}
|
||||||
|
SlTransform::~SlTransform()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
void LD14_LiPkg::Transform(std::vector<PointData> &tmp)
|
||||||
|
{
|
||||||
|
SlTransform trans(LDVersion::LD_FOURTEENTH);
|
||||||
|
tmp = trans.Transform(tmp);
|
||||||
|
// std::cout << "Transform LD_FOURTEENTH !!" << std::endl;
|
||||||
|
}
|
||||||
174
lidar.h
Normal file
174
lidar.h
Normal file
@ -0,0 +1,174 @@
|
|||||||
|
/**
|
||||||
|
* @file lidar.h
|
||||||
|
* @author LD Robot
|
||||||
|
* @version V01
|
||||||
|
|
||||||
|
* @brief
|
||||||
|
* @note
|
||||||
|
* @attention COPYRIGHT LDROBOT
|
||||||
|
**/
|
||||||
|
|
||||||
|
#ifndef __LIPKG_H
|
||||||
|
#define __LIPKG_H
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <vector>
|
||||||
|
#include <array>
|
||||||
|
#include <iostream>
|
||||||
|
#include <functional>
|
||||||
|
#include <thread>
|
||||||
|
#include <atomic>
|
||||||
|
#include <inttypes.h>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <sys/file.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <termios.h>
|
||||||
|
#include <memory.h>
|
||||||
|
#include <libudev.h>
|
||||||
|
#include<opencv2/opencv.hpp>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
struct PointData
|
||||||
|
{
|
||||||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>ʽ
|
||||||
|
float angle;
|
||||||
|
uint16_t distance;
|
||||||
|
uint8_t confidence;
|
||||||
|
//ֱ<><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>ʽ
|
||||||
|
double x;
|
||||||
|
double y;
|
||||||
|
PointData(float angle, uint16_t distance, uint8_t confidence, double x = 0, double y = 0)
|
||||||
|
{
|
||||||
|
this->angle = angle;
|
||||||
|
this->distance = distance;
|
||||||
|
this->confidence = confidence;
|
||||||
|
this->x = x;
|
||||||
|
this->y = y;
|
||||||
|
}
|
||||||
|
PointData() {}
|
||||||
|
friend std::ostream& operator<<(std::ostream& os, const PointData& data)
|
||||||
|
{
|
||||||
|
os << data.angle << " " << data.distance << " " << (int)data.confidence << " " << data.x << " " << data.y;
|
||||||
|
return os;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
typedef std::vector<PointData> Points2D;
|
||||||
|
|
||||||
|
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
PKG_HEADER = 0x54,
|
||||||
|
PKG_VER_LEN = 0x2C,
|
||||||
|
POINT_PER_PACK = 12,
|
||||||
|
};
|
||||||
|
typedef struct __attribute__((packed))
|
||||||
|
{
|
||||||
|
uint16_t distance;
|
||||||
|
uint8_t confidence;
|
||||||
|
}LidarPointStructDef;
|
||||||
|
typedef struct __attribute__((packed))
|
||||||
|
{
|
||||||
|
uint8_t header;
|
||||||
|
uint8_t ver_len;
|
||||||
|
uint16_t speed;
|
||||||
|
uint16_t start_angle;
|
||||||
|
LidarPointStructDef point[POINT_PER_PACK];
|
||||||
|
uint16_t end_angle;
|
||||||
|
uint16_t timestamp;
|
||||||
|
uint8_t crc8;
|
||||||
|
}LiDARFrameTypeDef;
|
||||||
|
struct FrameData
|
||||||
|
{
|
||||||
|
ros::Time timestamp;
|
||||||
|
float angle_min;
|
||||||
|
float angle_max;
|
||||||
|
uint32_t len;
|
||||||
|
std::vector<uint16_t> distance;
|
||||||
|
std::vector<uint8_t> intensities;
|
||||||
|
};
|
||||||
|
class LiPkg
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
LiPkg();
|
||||||
|
double GetSpeed(void); /*Lidar spin speed (Hz)*/
|
||||||
|
uint16_t GetTimestamp(void) { return mTimestamp; } /*time stamp of the packet */
|
||||||
|
bool IsPkgReady(void) { return mIsPkgReady; }/*a packet is ready */
|
||||||
|
bool IsFrameReady(void) { return mIsFrameReady; }/*Lidar data frame is ready*/
|
||||||
|
long GetErrorTimes(void) { return mErrorTimes; }/*the number of errors in parser process of lidar data frame*/
|
||||||
|
const std::array<PointData, POINT_PER_PACK>& GetPkgData(void);/*original data package*/
|
||||||
|
bool Parse(const uint8_t* data, long len);/*parse single packet*/
|
||||||
|
virtual void Transform(std::vector<PointData>& tmp) = 0;/*transform raw data to stantard data */
|
||||||
|
bool AssemblePacket();/*combine stantard data into data frames and calibrate*/
|
||||||
|
const FrameData& GetFrameData(void) { mIsFrameReady = false; return mFrameData; }
|
||||||
|
std::vector<cv::Mat> scan_to_pointcloud(FrameData mFrameData);
|
||||||
|
private:
|
||||||
|
uint16_t mTimestamp;
|
||||||
|
double mSpeed;
|
||||||
|
std::vector<uint8_t> mDataTmp;
|
||||||
|
long mErrorTimes;
|
||||||
|
std::array<PointData, POINT_PER_PACK>mOnePkg;
|
||||||
|
std::vector<PointData> mFrameTemp;
|
||||||
|
FrameData mFrameData;
|
||||||
|
bool mIsPkgReady;
|
||||||
|
bool mIsFrameReady;
|
||||||
|
};
|
||||||
|
class LD14_LiPkg : public LiPkg
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual void Transform(std::vector<PointData>& tmp);
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class LDVersion
|
||||||
|
{
|
||||||
|
LD_ZERO, /*Zero generation lidar*/
|
||||||
|
LD_THREE, /*Third generation lidar*/
|
||||||
|
LD_EIGHT, /*Eight generation radar*/
|
||||||
|
LD_NINE, /*Nine generation radar*/
|
||||||
|
LD_FOURTEENTH /*Fourteenth generation radar*/
|
||||||
|
};
|
||||||
|
|
||||||
|
class SlTransform
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
bool to_right_hand = true;
|
||||||
|
double offset_x;
|
||||||
|
double offset_y;
|
||||||
|
|
||||||
|
public:
|
||||||
|
SlTransform(LDVersion version, bool to_right_hand = false);
|
||||||
|
Points2D Transform(const Points2D& data);
|
||||||
|
~SlTransform();
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class CmdInterfaceLinux
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
CmdInterfaceLinux(int32_t ver);
|
||||||
|
~CmdInterfaceLinux();
|
||||||
|
|
||||||
|
bool Open(std::string& port_name);
|
||||||
|
bool Close();
|
||||||
|
bool ReadFromIO(uint8_t* rx_buf, uint32_t rx_buf_len, uint32_t* rx_len);
|
||||||
|
bool WriteToIo(const uint8_t* tx_buf, uint32_t tx_buf_len, uint32_t* tx_len);
|
||||||
|
bool GetCmdDevices(std::vector<std::pair<std::string, std::string> >& device_list);
|
||||||
|
void SetReadCallback(std::function<void(const char*, size_t length)> callback) { mReadCallback = callback; }
|
||||||
|
bool IsOpened() { return mIsCmdOpened.load(); };
|
||||||
|
bool IsExited() { return mRxThreadExitFlag.load(); };
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::thread* mRxThread;
|
||||||
|
static void mRxThreadProc(void* param);
|
||||||
|
long long mRxCount;
|
||||||
|
int32_t version;
|
||||||
|
int32_t mComHandle;
|
||||||
|
std::atomic<bool> mIsCmdOpened, mRxThreadExitFlag;
|
||||||
|
std::function<void(const char*, size_t length)> mReadCallback;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/
|
||||||
167
main.cpp
Normal file
167
main.cpp
Normal file
@ -0,0 +1,167 @@
|
|||||||
|
#include <iostream>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "lidar.h"
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/LaserScan.h>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include "ranging.h"
|
||||||
|
#include "fusion.h"
|
||||||
|
#include <string>
|
||||||
|
#include <pthread.h>
|
||||||
|
#include <mutex>
|
||||||
|
#include <sys/time.h>
|
||||||
|
/*#include <cv_bridge/cv_bridge.h>
|
||||||
|
#include <image_transport/image_transport.h>*/
|
||||||
|
#include "opencv2/imgcodecs/imgcodecs.hpp"
|
||||||
|
#include <ctime>
|
||||||
|
|
||||||
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
||||||
|
|
||||||
|
Ranging r(9, 640, 480);
|
||||||
|
std::queue<std::vector<Mat>> frameInfo;
|
||||||
|
std::queue<FrameData> laser_queue;
|
||||||
|
std::mutex g_mutex;
|
||||||
|
|
||||||
|
void *ranging(void *args) // ranging线程
|
||||||
|
{
|
||||||
|
while (true)
|
||||||
|
{
|
||||||
|
std::cout<<" ************ enter ranging *********** "<<std::endl;
|
||||||
|
std::vector<Mat> result = r.get_range();
|
||||||
|
g_mutex.lock();
|
||||||
|
for (uchar i = 0; i < frameInfo.size(); i++) // 只保存当前最新的图片帧信息
|
||||||
|
frameInfo.pop();
|
||||||
|
frameInfo.push(result);
|
||||||
|
g_mutex.unlock();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char **argv)
|
||||||
|
{
|
||||||
|
uint8_t laser_queue_size = 2; // 雷达帧队列大小参数
|
||||||
|
LD14_LiPkg *pkg = new LD14_LiPkg;
|
||||||
|
CmdInterfaceLinux cmd_port(9);
|
||||||
|
std::string port_name = "/dev/ttyUSB0";
|
||||||
|
if (argc > 1)
|
||||||
|
port_name = argv[1];
|
||||||
|
// 雷达读取回调函数，一直在执行雷达读取和处理数据
|
||||||
|
cmd_port.SetReadCallback([&pkg](const char *byte, size_t len) {
|
||||||
|
if (pkg->Parse((uint8_t*)byte, len))
|
||||||
|
{
|
||||||
|
pkg->AssemblePacket();
|
||||||
|
} });
|
||||||
|
if (!cmd_port.Open(port_name))
|
||||||
|
return -1;
|
||||||
|
int type = VideoWriter::fourcc('M','J','P','G');
|
||||||
|
char name[256] = {0};
|
||||||
|
FILE *fp=NULL;
|
||||||
|
time_t timep;
|
||||||
|
struct tm *p;
|
||||||
|
time(&timep);
|
||||||
|
p = localtime(&timep);
|
||||||
|
sprintf(name,"%d.%d.%d %d:%02d.avi",1900+p->tm_year,1+p->tm_mon,p->tm_mday,p->tm_hour,p->tm_min);
|
||||||
|
VideoWriter writer(name, type, 30, Size(640, 480), true);
|
||||||
|
ros::init(argc, argv, "publish");
|
||||||
|
ros::NodeHandle nh; /* create a ROS Node */
|
||||||
|
ros::Publisher uplidar_pub = nh.advertise<sensor_msgs::LaserScan>("up_scan", 1);
|
||||||
|
//ros::Publisher downlidar_pub = nh.advertise<sensor_msgs::LaserScan>("down_scan", 1);
|
||||||
|
sensor_msgs::LaserScan upscan;
|
||||||
|
// sensor_msgs::LaserScan downscan;
|
||||||
|
|
||||||
|
|
||||||
|
/*image_transport::ImageTransport it(nh);
|
||||||
|
image_transport::Publisher img_pub = it.advertise("camera/image", 1);*/
|
||||||
|
pthread_t tids[1]; // 执行ranging线程
|
||||||
|
int ret = pthread_create(&tids[0], NULL, ranging, NULL);
|
||||||
|
if (ret != 0)
|
||||||
|
{
|
||||||
|
std::cout << "Multithreading error !" << std::endl;
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
|
||||||
|
LCFusion lcFusion(&r);
|
||||||
|
namedWindow("camera");
|
||||||
|
|
||||||
|
while (ros::ok() && !cmd_port.IsExited())
|
||||||
|
{
|
||||||
|
int64 t = getTickCount();
|
||||||
|
if (pkg->IsFrameReady()) // 若一帧雷达处理好，执行后续操作
|
||||||
|
{
|
||||||
|
FrameData laserData_queue = pkg->GetFrameData(); // 得到雷达数据
|
||||||
|
laserData_queue.timestamp = ros::Time::now();
|
||||||
|
laser_queue.push(laserData_queue);
|
||||||
|
while(laser_queue.size() > laser_queue_size) // 只保存两帧雷达数据
|
||||||
|
{
|
||||||
|
laser_queue.pop();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
continue;
|
||||||
|
if (laser_queue.size() == laser_queue_size) // 若队列有两帧数据，执行后续操作
|
||||||
|
{
|
||||||
|
FrameData laserData = laser_queue.front();
|
||||||
|
laser_queue.pop();
|
||||||
|
upscan.header.stamp = laserData.timestamp;
|
||||||
|
// upscan.header.stamp = ros::Time::now();
|
||||||
|
// downscan.header.stamp = upscan.header.stamp;
|
||||||
|
|
||||||
|
// uint16_t times_stamp = pkg->GetTimestamp();
|
||||||
|
// FrameData laserData = pkg->GetFrameData();
|
||||||
|
char state = -1;
|
||||||
|
|
||||||
|
float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); // 角度弧度转换
|
||||||
|
float angle_max = ANGLE_TO_RADIAN(laserData.angle_max);
|
||||||
|
uint32_t len = laserData.len;
|
||||||
|
float angle_inc = (angle_max - angle_min) / (len - 1.); // 角度增量
|
||||||
|
lcFusion.filter_laser(laserData, angle_min, angle_inc); // 视角同步，处理视角内雷达数据
|
||||||
|
if (!frameInfo.empty())
|
||||||
|
{
|
||||||
|
g_mutex.lock();
|
||||||
|
std::vector<Mat> camInfo = frameInfo.front(); // 读取图像帧数据
|
||||||
|
frameInfo.pop();
|
||||||
|
g_mutex.unlock();
|
||||||
|
double fusion_old,fusion_now;
|
||||||
|
fusion_old = ros::Time::now().toSec();
|
||||||
|
state = lcFusion.fusion(camInfo); // 融合
|
||||||
|
fusion_now = ros::Time::now().toSec();
|
||||||
|
cout << "data_fusion: " << fusion_now - fusion_old << endl;
|
||||||
|
if (state != -2)
|
||||||
|
{
|
||||||
|
lcFusion.set_laser_data(upscan); // 设置雷达发布数据
|
||||||
|
uplidar_pub.publish(upscan); // 发布
|
||||||
|
}
|
||||||
|
// 调试使用，将雷达点映射到图像中显示
|
||||||
|
if (!lcFusion.projectedPoints.empty())
|
||||||
|
{
|
||||||
|
for (int i = 0; i < lcFusion.projectedPoints.size(); i++)
|
||||||
|
{
|
||||||
|
Point2f p = lcFusion.projectedPoints[i];
|
||||||
|
circle(camInfo[0], p, 3, Scalar(0, 255, 0), 1, 8);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/* sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", camInfo[0]).toImageMsg();
|
||||||
|
img_pub.publish(msg);*/
|
||||||
|
if (!camInfo[0].empty())
|
||||||
|
{
|
||||||
|
cv::Mat pic;
|
||||||
|
cv::Size dsize= Size(int(640*0.75),int(480*0.75));
|
||||||
|
cv::resize(camInfo[0],pic,dsize,0,0,INTER_AREA);
|
||||||
|
// cv::pyrDown(camInfo[0],pic);
|
||||||
|
writer.write(camInfo[0]);
|
||||||
|
imshow("camera", pic);
|
||||||
|
}
|
||||||
|
t = getTickCount() - t;
|
||||||
|
char fps[50];
|
||||||
|
std::cout << "fps: " << int(1 / (t / getTickFrequency())) << std::endl;
|
||||||
|
if (waitKey(1) == 81)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
//lcFusion.set_laser_data(downscan, laserData);
|
||||||
|
//downlidar_pub.publish(downscan);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
std::cout << "ROS or lidar error !" << std::endl;
|
||||||
|
delete pkg;
|
||||||
|
cmd_port.Close();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
444
ranging.cpp
Normal file
444
ranging.cpp
Normal file
@ -0,0 +1,444 @@
|
|||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "ranging.h"
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
#include "rknn_sdk.h"
|
||||||
|
#include <string.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
#include <dlfcn.h>
|
||||||
|
#include "opencv2/core.hpp"
|
||||||
|
#include "opencv2/imgcodecs.hpp"
|
||||||
|
#include "opencv2/imgproc.hpp"
|
||||||
|
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <dirent.h>
|
||||||
|
#include <vector>
|
||||||
|
#include <tuple>
|
||||||
|
#include <string>
|
||||||
|
#include <string.h>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
using namespace cv;
|
||||||
|
|
||||||
|
|
||||||
|
void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR) //重映射函数
|
||||||
|
{
|
||||||
|
remap(oriImgL, oriImgL, mapX1, mapX2, INTER_LINEAR);
|
||||||
|
remap(oriImgR, oriImgR, mapY1, mapY2, INTER_LINEAR);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
|
||||||
|
{
|
||||||
|
//(u,v)->(x,y)"(loc[0],loc[1])"
|
||||||
|
std::vector<float> loc;
|
||||||
|
loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
|
||||||
|
loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
|
||||||
|
return loc;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<int> Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates) //模板匹配
|
||||||
|
{
|
||||||
|
int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
|
||||||
|
// cv::Mat right_image_,left_image_;
|
||||||
|
// cv::pyrDown(right_image, right_image_);
|
||||||
|
// cv::pyrDown(left_image, left_image_);
|
||||||
|
Mat tpl = right_image.rowRange(max(y1 - 2, 0), min(y2 + 2, 479)).colRange(x1, x2); //获取目标框
|
||||||
|
Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); //待匹配图像，极线约束，只需要同水平区域
|
||||||
|
// Mat tpl = right_image_.rowRange(max(int(0.5*y1 - 2), 0), min(int(0.5*y2 + 2), 479)).colRange(int(0.5*x1), int(0.5*x2));
|
||||||
|
// Mat target = left_image_.rowRange(max(int(0.5*y1 - 10), 0), min(int(0.5*y2 + 10), 239)).colRange(0, 319);
|
||||||
|
int th = tpl.rows, tw = tpl.cols;
|
||||||
|
Mat result;
|
||||||
|
// methods = TM_CCOEFF_NORMED, TM_SQDIFF_NORMED, TM_CCORR_NORMED]
|
||||||
|
matchTemplate(target, tpl, result, TM_CCOEFF_NORMED); //匹配方法：归一化相关系数即零均值归一化互相关
|
||||||
|
double minVal, maxVal;
|
||||||
|
Point minLoc, maxLoc;
|
||||||
|
minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc); //得到匹配点坐标
|
||||||
|
Point tl = maxLoc, br;
|
||||||
|
|
||||||
|
br.x = min(maxLoc.x + tw, 639); //转为像素坐标系
|
||||||
|
br.y = min(maxLoc.y + th, 479); //转为像素坐标系
|
||||||
|
////展示匹配结果
|
||||||
|
// br.x = min(maxLoc.x + tw, 319);
|
||||||
|
// br.y = min(maxLoc.y + th, 239);
|
||||||
|
//rectangle(target, tl, br, (0, 255, 0), 3);
|
||||||
|
//imshow("match-", target);
|
||||||
|
//waitKey(2);
|
||||||
|
std::vector<int> maxloc;
|
||||||
|
maxloc.push_back(maxLoc.x);
|
||||||
|
maxloc.push_back(maxLoc.y);
|
||||||
|
return maxloc;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ranging::horizon_estimate(Mat& img, Mat& bboxs,int k)
|
||||||
|
{
|
||||||
|
//保证摄像头与地面平行
|
||||||
|
int x1 = bboxs.at<float>(k, 0);
|
||||||
|
int x2 = bboxs.at<float>(k, 2);
|
||||||
|
int y1 = bboxs.at<float>(k, 1);
|
||||||
|
int y2 = bboxs.at<float>(k, 3);
|
||||||
|
float Conf = bboxs.at<float>(k, 4);
|
||||||
|
int cls = bboxs.at<float>(k, 5);
|
||||||
|
float Y_B, Y_H;
|
||||||
|
Mat edge, grayImage;
|
||||||
|
vector<cv::Point> idx;
|
||||||
|
Mat tpl = img.rowRange(y1, min(y2+5,479)).colRange(x1, x2); // 取感兴趣范围
|
||||||
|
//Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639);
|
||||||
|
Mat Z = Mat::zeros(2, tpl.cols, CV_32FC1);
|
||||||
|
cvtColor(tpl, grayImage, COLOR_BGR2GRAY);
|
||||||
|
GaussianBlur(grayImage,grayImage,Size(5,5),0);
|
||||||
|
Canny(grayImage, edge, 120, 180, 3); //提取边缘，获取与地面接触点
|
||||||
|
//cv::imshow("1",edge);
|
||||||
|
//cv::waitKey(1);
|
||||||
|
float cluster[650];
|
||||||
|
for (int i = 0;i<650;i++)
|
||||||
|
{
|
||||||
|
cluster[i] = 0;
|
||||||
|
}
|
||||||
|
int y_b, y_h;
|
||||||
|
int j = 0;
|
||||||
|
for (int i = 0; i < x2-x1; i++)
|
||||||
|
{
|
||||||
|
//Mat temp = edge.rowRange(max(y1, 0), min(y2 + 4, 479)).colRange(x1, x2);
|
||||||
|
Mat temp = edge.col(i); //取第i列
|
||||||
|
// std::cout << "temp: " <<temp << std::endl;
|
||||||
|
cv::findNonZero(temp, idx);
|
||||||
|
std::vector<float> point_b = pic2cam(x1 + i, 240); //转为相机坐标系
|
||||||
|
std::vector<float> point_H = pic2cam(320, 240);
|
||||||
|
float alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
|
||||||
|
if (idx.size() < 1)
|
||||||
|
{
|
||||||
|
Z.at<float>(0, i) = 0;
|
||||||
|
Z.at<float>(1, i) = alfa;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
int y_b = idx[idx.size() - 1].y + y1; //
|
||||||
|
y_b = int(y_b + y_b*0.03);
|
||||||
|
int y_h = 240;
|
||||||
|
point_b = pic2cam(x1 + i, y_b); //转为相机坐标系
|
||||||
|
point_H = pic2cam(320, y_h);
|
||||||
|
Y_B = point_b[1];
|
||||||
|
Y_H = point_H[1];
|
||||||
|
// std::cout << "y_b: " << y_b << std::endl;
|
||||||
|
float H_c = 60; //摄像头离地高度，单位mm
|
||||||
|
float theta = 0; //摄像头与地面夹角，弧度
|
||||||
|
float d = (1/cos(theta)* cos(theta)) * q.at<double>(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta);
|
||||||
|
alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
|
||||||
|
//cout << "d: " << d << endl;
|
||||||
|
if (d > 700)
|
||||||
|
{d = 0;}
|
||||||
|
Z.at<float>(0, i) = d/cos(alfa);
|
||||||
|
Z.at<float>(1, i) = alfa;
|
||||||
|
}
|
||||||
|
/*if (i > 0)
|
||||||
|
{
|
||||||
|
if (abs(Z.at<float>(0, i) - Z.at<float>(0, i - 1)) < 40) //聚类
|
||||||
|
{
|
||||||
|
cluster[j] = cluster[j] + 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
j = j + 1;
|
||||||
|
cluster[j] = 1;
|
||||||
|
//std::cout<<"j : "<< j<< std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//int max_loc = distance(cluster,max_element(cluster,cluster + sizeof(cluster)/sizeof(cluster[0]))); //只保留数量最多的一类，其余置0
|
||||||
|
//std::cout<<" max_loc : "<< max_loc<<std::endl;
|
||||||
|
int temp = 0;
|
||||||
|
for (int i = 0; i < j; i++)
|
||||||
|
{
|
||||||
|
if (cluster[i] < 3)
|
||||||
|
{
|
||||||
|
for (int t = temp; t < temp + cluster[i]; t++)
|
||||||
|
{
|
||||||
|
Z.at<float>(0, t) = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
temp = temp + cluster[i];
|
||||||
|
}*/
|
||||||
|
// std::cout << "z : " <<Z << std::endl;
|
||||||
|
this->Z = Z.clone();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
|
||||||
|
{
|
||||||
|
// ֱ<><D6B1>ͼ<EFBFBD><CDBC><EFBFBD>⻯
|
||||||
|
Mat imgGrayL, imgGrayR;
|
||||||
|
cvtColor(imgL, imgGrayL, COLOR_BGR2GRAY);
|
||||||
|
cvtColor(imgR, imgGrayR, COLOR_BGR2GRAY);
|
||||||
|
Mat imgR_weight = imgR.clone();
|
||||||
|
Mat infoRow;
|
||||||
|
for (uchar i = 0; i < detBoxes.rows; i++)
|
||||||
|
{
|
||||||
|
int x1 = detBoxes.at<float>(i, 0);
|
||||||
|
int y1 = detBoxes.at<float>(i, 1);
|
||||||
|
int x2 = detBoxes.at<float>(i, 2);
|
||||||
|
int y2 = detBoxes.at<float>(i, 3);
|
||||||
|
//std::cout<<"x1: "<<x1<<"x2: "<<x2<<"y1: "<<y1<<"y2: "<<y2<<std::endl;
|
||||||
|
float Conf = detBoxes.at<float>(i, 4);
|
||||||
|
int cls = detBoxes.at<float>(i, 5);
|
||||||
|
|
||||||
|
if (cls == 6 || cls == 10 || cls == 11) //体重秤，地毯和毛巾采用单目测距方法
|
||||||
|
{
|
||||||
|
horizon_estimate(imgR_weight, detBoxes, i);
|
||||||
|
}
|
||||||
|
if (x1 > 600 || x2 < 50 || y1 < 5 || y2 > 475 || x1 < 2 || x2 > 590 || abs(x2 - x1) > 550) //当目标框偏左、偏右或者过大，略去该物体
|
||||||
|
{
|
||||||
|
|
||||||
|
//char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
rectangle(imgR, Point(int(x1), int(y1)),
|
||||||
|
Point(int(x2), int(y2)), Scalar(0, 0, 255));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (cls!=0 && cls!=1 && cls!=2 && cls!= 6 && cls!= 7 && cls!= 10 && cls!=11 && cls!=14 && cls!=15) //大物体不进行测距
|
||||||
|
{
|
||||||
|
if (x1 < 30 || x2 < 80 || x1>580 || x2 > 610) //删除边缘的目标框
|
||||||
|
{
|
||||||
|
detBoxes.at<float>(i, 5) = -1;
|
||||||
|
cls = detBoxes.at<float>(i, 5);
|
||||||
|
}
|
||||||
|
//char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
rectangle(imgR, Point(int(x1), int(y1)),
|
||||||
|
Point(int(x2), int(y2)), Scalar(0, 0, 255));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
rectangle(imgR, Point(int(x1), int(y1)),
|
||||||
|
Point(int(x2), int(y2)), Scalar(0, 0, 255)); //绘制目标框
|
||||||
|
|
||||||
|
int coordinates[4] = {x1, y1, x2, y2};
|
||||||
|
std::vector<int> disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配
|
||||||
|
float disp_pixel_x = disp_pixel[0] - x1; //ˮ计算水平视差
|
||||||
|
float disp_pixel_y = disp_pixel[1] - y1; //
|
||||||
|
disp_pixel_x = (int)(disp_pixel_x + disp_pixel_x * 0.12); //0.12为模板匹配产生的误差，为经验值，通过拟合得到
|
||||||
|
|
||||||
|
//Mat disp_matrix = Mat(1, 1, CV_32F, Scalar(disp_pixel_x)), disp_pixel_xyz;
|
||||||
|
Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x)); //定义视差矩阵，所有值均为水平视差，方便转换为三维坐标，并具有水平距离信息
|
||||||
|
Mat threed_pixel_xyz, threedImage;
|
||||||
|
reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d
|
||||||
|
threed_pixel_xyz = threedImage.mul(threedImage); //每一像素点求平方，
|
||||||
|
std::vector<Mat> channels;
|
||||||
|
split(threed_pixel_xyz.clone(), channels);
|
||||||
|
threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //计算欧式距离
|
||||||
|
threed_pixel_xyz.forEach<float>([](float &value, const int *position) { value = sqrt(value); }); // 获得距离d
|
||||||
|
|
||||||
|
int mid_pixel = int((x1 + x2) / 2);
|
||||||
|
std::vector<float> mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //计算角度，从像素坐标转为相机坐标
|
||||||
|
std::vector<float> loc_tar = pic2cam(mid_pixel, imgGrayR.rows);
|
||||||
|
float alfa = atan((loc_tar[0] - mid[0]) / q.at<double>(2, 3));
|
||||||
|
|
||||||
|
|
||||||
|
if (disp_pixel_x > 240) // 距离太近，视差过大
|
||||||
|
{
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
float median = threed_pixel_xyz.at<float>((int)(y1 + y2) / 2, (int)(x1 + x2) / 2);
|
||||||
|
|
||||||
|
std::vector<float> ltPoint = pic2cam(x1, y1);
|
||||||
|
std::vector<float> rbPoint = pic2cam(x2, y2);
|
||||||
|
float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //计算宽高
|
||||||
|
float f = q.at<double>(2, 3);
|
||||||
|
float f1 = sqrt(xx1 * xx1 + yy1 * yy1 + f * f); //推导得出
|
||||||
|
//float w1 = median * sqrt((xx1 - xx2) * (xx1 - xx2) / 4) / f1;
|
||||||
|
float h1 = median * sqrt((yy1 - yy2) * (yy1 - yy2) / 4) / f1;
|
||||||
|
float f2 = sqrt(xx2 * xx2 + yy2 * yy2 + f * f);
|
||||||
|
//float w2 = median * sqrt((xx2 - xx1) * (xx2 - xx1) / 4) / f2;
|
||||||
|
float h2 = median * sqrt((yy2 - yy1) * (yy2 - yy1) / 4) / f2;
|
||||||
|
float w1 = sqrt(pow((threedImage.at<cv::Vec3f>(y2, x1)[0] - threedImage.at<cv::Vec3f>(y2, x2)[0]), 2) +
|
||||||
|
pow((threedImage.at<cv::Vec3f>(y2, x1)[1] - threedImage.at<cv::Vec3f>(y2, x2)[1]), 2) +
|
||||||
|
pow((threedImage.at<cv::Vec3f>(y2, x1)[2] - threedImage.at<cv::Vec3f>(y2, x2)[2]), 2));
|
||||||
|
|
||||||
|
w1 = w1 / 10;
|
||||||
|
h1 = (h1 + h2) / 10;
|
||||||
|
median /= 10;
|
||||||
|
if (median > 120) //过远测距误差较大
|
||||||
|
{
|
||||||
|
//char tf[9];
|
||||||
|
//sprintf(tf, "Too far!");
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
// <20><>ͼ<EFBFBD><CDBC><EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
||||||
|
char dc[50], wh[50];
|
||||||
|
std::string cname = className[cls + 1];
|
||||||
|
sprintf(dc, "dis:%.2fcm %d", median, cls);
|
||||||
|
sprintf(wh, "W: %.2fcm H: %.2fcm alfa: %2f", w1, h1, alfa);
|
||||||
|
putText(imgR, dc, Point(x1, y2), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 2);
|
||||||
|
putText(imgR, wh, Point(x1, y1), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 1.5);
|
||||||
|
|
||||||
|
|
||||||
|
//返回数据
|
||||||
|
infoRow = (Mat_<float>(1, 4) << median, w1, h1, alfa);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
};
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Ranging::Ranging(int index, int imgw, int imgh) : //初始化
|
||||||
|
mapX1(imgh, imgw, CV_64F), //初始化矩阵 ，用于计算无畸变和修正转换映射。
|
||||||
|
mapX2(imgh, imgw, CV_64F),
|
||||||
|
mapY1(imgh, imgw, CV_64F),
|
||||||
|
mapY2(imgh, imgw, CV_64F),
|
||||||
|
q(4, 4, CV_64F),
|
||||||
|
imgw(imgw),
|
||||||
|
imgh(imgh)
|
||||||
|
{
|
||||||
|
// Z = Mat::zeros(2, 1, CV_32FC1);
|
||||||
|
if (!vcapture.open(index))
|
||||||
|
{
|
||||||
|
std::cout << "Open camera failed !" << std::endl;
|
||||||
|
exit(EXIT_FAILURE);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
//vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG);
|
||||||
|
vcapture.set(CAP_PROP_FPS, 30);
|
||||||
|
vcapture.set(CAP_PROP_FRAME_WIDTH, imgw * 2);
|
||||||
|
vcapture.set(CAP_PROP_FRAME_HEIGHT, imgh);
|
||||||
|
vcapture.set(CAP_PROP_BUFFERSIZE, 1);
|
||||||
|
|
||||||
|
auto imgSize = Size(imgw, imgh);
|
||||||
|
Mat r1(3, 3, CV_64F), r2(3, 3, CV_64F), p1(3, 4, CV_64F), p2(3, 4, CV_64F);
|
||||||
|
|
||||||
|
stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r,
|
||||||
|
imgSize, rotate.t(), trans, r1, r2, p1, p2, q);//立体校正
|
||||||
|
|
||||||
|
initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);//计算无畸变和修正转换映射
|
||||||
|
initUndistortRectifyMap(cam_matrix_right.t(), distortion_r, r2, p2, imgSize, CV_32F, mapY1, mapY2);//计算无畸变和修正转换映射
|
||||||
|
|
||||||
|
RKNN_Create(&hdx, modelPath); // 初始化检测模型
|
||||||
|
std::cout<< " ******************* CAMERA initialization ********************" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
|
||||||
|
// {
|
||||||
|
// std::vector<float> loc;
|
||||||
|
// loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
|
||||||
|
// loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
|
||||||
|
// return loc;
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
std::vector<Mat> Ranging::get_range()
|
||||||
|
{
|
||||||
|
double rang_old, rang_now;
|
||||||
|
rang_old = ros::Time::now().toSec(); //测试运行时间
|
||||||
|
Mat frame, lframe, rframe;
|
||||||
|
vcapture.read(frame); //获取视频帧
|
||||||
|
|
||||||
|
if (!frame.empty())
|
||||||
|
{
|
||||||
|
int64 t = getTickCount();
|
||||||
|
Mat lframe(frame.colRange(0, imgw).clone()); //拷贝左图
|
||||||
|
Mat rframe(frame.colRange(imgw, imgw * 2).clone()); //拷贝右图
|
||||||
|
//imshow("lframe",lframe);
|
||||||
|
//waitKey(1);
|
||||||
|
rectifyImage(lframe, rframe); //
|
||||||
|
|
||||||
|
DetRet *det_ret = NULL; //
|
||||||
|
InputData input_data; //定义输入数据
|
||||||
|
int shape[4] = {1, 3, rframe.rows, rframe.cols}, det_num = 0;
|
||||||
|
memcpy(input_data.shape, shape, sizeof(shape));
|
||||||
|
Mat Rframe = rframe.clone();
|
||||||
|
|
||||||
|
double detect_old, detect_now;
|
||||||
|
detect_old = ros::Time::now().toSec();
|
||||||
|
input_data.data = Rframe.data;
|
||||||
|
//std::cout<<"Rframe.data.shape"<<Rframe.rows<<std::endl;
|
||||||
|
RKNN_ObjDet(hdx, &input_data, &det_ret, &det_num);
|
||||||
|
detect_now = ros::Time::now().toSec();
|
||||||
|
|
||||||
|
//cout << "data_detect_time:" << detect_now-detect_old << endl;
|
||||||
|
//std::cout<<"det_num "<<det_num<<std::endl;
|
||||||
|
if (!det_num)
|
||||||
|
{
|
||||||
|
//std::cout<<"return NULL"<<std::endl;
|
||||||
|
return std::vector<Mat>{rframe}; //没有检测框时，退出
|
||||||
|
}
|
||||||
|
std::cout << "det_num: " << det_num << std::endl;
|
||||||
|
// detction box transfor to our format
|
||||||
|
Mat detBoxes(det_num, 6, CV_32F); //定义矩阵，存储目标检测内容，存储格式(x,y,x,y,conf,cls)
|
||||||
|
for (int i = 0; i < det_num; i++) //存储目标检测内容 (x,y,x,y,conf,cls)
|
||||||
|
{
|
||||||
|
DetRet det_result = det_ret[i];
|
||||||
|
// xyxy conf cls
|
||||||
|
float xmin = rframe.cols * det_result.fLeft;
|
||||||
|
float ymin = rframe.rows * det_result.fTop;
|
||||||
|
float xmax = rframe.cols * det_result.fRight;
|
||||||
|
float ymax = rframe.rows * det_result.fBottom;
|
||||||
|
|
||||||
|
detBoxes.at<float>(i, 0) = xmin;
|
||||||
|
detBoxes.at<float>(i, 1) = ymin;
|
||||||
|
detBoxes.at<float>(i, 2) = xmax;
|
||||||
|
detBoxes.at<float>(i, 3) = ymax;
|
||||||
|
detBoxes.at<float>(i, 4) = det_result.fConf;
|
||||||
|
// 实验测试，过滤过大的误检框
|
||||||
|
float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
|
||||||
|
if (ratio > 0.7)
|
||||||
|
{
|
||||||
|
detBoxes.at<float>(i, 5) = -1;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
detBoxes.at<float>(i, 5) = det_result.nLabel;
|
||||||
|
if (det_result.nLabel == 1 || det_result.nLabel == 2) //检测目标为体重秤和风扇底座时，将目标框拉高，使杆子的雷达点能与目标框对应
|
||||||
|
{
|
||||||
|
detBoxes.at<float>(i, 1) = 100;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
Mat info(det_num, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α
|
||||||
|
if (det_num)
|
||||||
|
{
|
||||||
|
// double rang_old, rang_now;
|
||||||
|
// rang_old = ros::Time::now().toSec();
|
||||||
|
getInfo(lframe, rframe, detBoxes, info);
|
||||||
|
// rang_now = ros::Time::now().toSec();
|
||||||
|
// cout << "data_dis_time: " << rang_now-rang_old << endl;
|
||||||
|
}
|
||||||
|
t = getTickCount() - t;
|
||||||
|
char fps[50];
|
||||||
|
sprintf(fps, "fps: %d", int(1 / (t / getTickFrequency())));
|
||||||
|
putText(rframe, fps, Point(20, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 1.5);
|
||||||
|
// rang_now = ros::Time::now().toSec();
|
||||||
|
// cout << "data_rang_time" << rang_now - rang_old << endl;
|
||||||
|
return std::vector<Mat>{rframe, detBoxes, info};
|
||||||
|
}
|
||||||
|
return std::vector<Mat>{rframe};
|
||||||
|
}
|
||||||
134
ranging.h
Normal file
134
ranging.h
Normal file
@ -0,0 +1,134 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/calib3d.hpp>
|
||||||
|
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <vector>
|
||||||
|
#include "rknn_sdk.h"
|
||||||
|
#include "fusion.h"
|
||||||
|
|
||||||
|
using namespace cv;
|
||||||
|
class LCFusion;
|
||||||
|
class Ranging
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
friend class LCFusion;
|
||||||
|
VideoCapture vcapture;
|
||||||
|
rknn_handle hdx;
|
||||||
|
const char *modelPath = "m28.rknn";
|
||||||
|
std::vector<std::string> className = {
|
||||||
|
"Unknown", //æªç¥
|
||||||
|
"MaoGouWan", //ç«ç¢/çç¢
|
||||||
|
"BTYDiZuo", //å§å°æ€
åºåº§
|
||||||
|
"FSDiZuo", //é£æåºåº§
|
||||||
|
"XiYiJi", //æŽè¡£æº
|
||||||
|
"BingXiang", //å°ç®±
|
||||||
|
"MaTong", //马桶
|
||||||
|
"TiZhongCheng", //äœé秀
|
||||||
|
"DianXian", //çµçº¿
|
||||||
|
"DianShiJi", //çµè§æº
|
||||||
|
"CanZhuo", //é€æ¡
|
||||||
|
"DiTan", //å°æ¯¯
|
||||||
|
"MaBu", //æ¹åž
|
||||||
|
"ChaJi", //è¶å
|
||||||
|
"DianShiGu", //çµè§æ
|
||||||
|
"XieZi", //æé/éå
|
||||||
|
"WaZi", //è¢å
|
||||||
|
"YiGui", //è¡£æ
|
||||||
|
"Chuang", //åº
|
||||||
|
"ShaFa", //æ²å
|
||||||
|
"YiZi", //æ€
å
|
||||||
|
"Men", //éš
|
||||||
|
"DLS", //倧çç³
|
||||||
|
"DiBan", //å°æ¿
|
||||||
|
};
|
||||||
|
|
||||||
|
// åç®åæ°
|
||||||
|
/*
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||||
|
4.700950170847520e+02, 0, 0,
|
||||||
|
0, 4.709244004725060e+02, 0,
|
||||||
|
3.220628601465352e+02, 2.265265667516499e+02, 1);
|
||||||
|
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||||
|
4.696016526986375e+02, 0, 0,
|
||||||
|
0, 4.702371464881610e+02, 0,
|
||||||
|
3.216994731643572e+02, 2.267449827655198e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) << 0.088023418049400, -0.047968406803285, 0,
|
||||||
|
0, 0);
|
||||||
|
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) << 0.097018738959281, -0.081707209740704, 0,
|
||||||
|
0, 0);
|
||||||
|
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||||||
|
0.999895538560624, 1.514179660491009e-04, 0.014452994124399,
|
||||||
|
-1.935031776135467e-04, 0.999995745723263, 0.002910514026079,
|
||||||
|
-0.014452491933249, -0.002913006689884, 0.999891314028152);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) <<
|
||||||
|
-59.978995009996730, 0.050886852679934, -0.088565378305175);*/
|
||||||
|
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||||
|
4.860907245880164e+02, 0, 0,
|
||||||
|
0, 4.899923700242200e+02, 0,
|
||||||
|
2.986108795628009e+02, 2.423211232345708e+02, 1);
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||||
|
4.833477325585679e+02, 0, 0,
|
||||||
|
0, 4.868370803438677e+02, 0,
|
||||||
|
2.976599964943864e+02, 2.310278327659757e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) << 0.126100629219048, -0.152357884696085, 0, 0, 0);
|
||||||
|
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) << 0.117495161060377, -0.131936017409618, 0, 0, 0);
|
||||||
|
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||||||
|
0.999964066385317, 0.001108038258875, 0.008404652839816,
|
||||||
|
-0.001043883137806, 0.999970316987207, -0.007633836027844,
|
||||||
|
-0.008412861946779, 0.007624788241143, 0.999935541101596);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) <<
|
||||||
|
-60.419864984062440, -0.059256838559736, -0.609155889994525);
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||||
|
3.627346593660044e+02, 0, 0,
|
||||||
|
0, 3.629168542317134e+02, 0,
|
||||||
|
3.093231127433517e+02, 2.409353074183058e+02, 1);
|
||||||
|
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||||
|
3.615909129180773e+02, 0, 0,
|
||||||
|
0, 3.617772267770084e+02, 0,
|
||||||
|
3.062363746645480e+02, 2.298948842159400e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) << 0.117219440226075, -0.148594691012994, 0,
|
||||||
|
0, 0);
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) << 0.116412295415583, -0.146817143572705, 0,
|
||||||
|
0, 0);
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||||||
|
0.999962353120719, 4.648106081008926e-04, 0.008664657660520,
|
||||||
|
-4.493098874759400e-04, 0.999998295540464, -0.001790820145135,
|
||||||
|
-0.008665475284162, 0.001786859609987, 0.999960857569352);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) <<
|
||||||
|
-60.097178758944190, -0.033859553542635, -0.423965574285253);
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) << 3.629995979572968e+02, 0, 0, 0, 3.605811984929196e+02, 0, 3.056618479641253e+02, 2.425978482096805e+02, 1);
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) << 3.642383320232306e+02, 0, 0, 0, 3.619176322068948e+02, 0, 3.048699917391443e+02, 2.355042537698953e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) << 0.114519742751324, -0.147768779338520, 0.001196745401736, -0.00274469994995, 0.008589264227682);
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) << 0.107862414325132, -0.112272107836791, 0.002046119288761, -8.703752362970271e-04, -0.050706452190122);
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) << 0.999984364022271, 6.994668898371339e-04, 0.005548194034452, -6.988446369697146e-04, 0.999999749299562, -1.140920137281147e-04, -0.005548272447104, 1.102129041420978e-04, 0.999984602144437);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) << -60.134851044411256, 0.007279986569326, -0.065092184396760);
|
||||||
|
*/
|
||||||
|
|
||||||
|
Mat mapX1, mapX2, mapY1, mapY2, q, Z;
|
||||||
|
|
||||||
|
int imgw, imgh;
|
||||||
|
|
||||||
|
public:
|
||||||
|
Ranging(int index, int imgw, int imgh);
|
||||||
|
void rectifyImage(Mat &oriImgL, Mat &oriImgR);
|
||||||
|
void getInfo(Mat &imgl, Mat &imgr, Mat &detBoxes, Mat &info);
|
||||||
|
std::vector<float> pic2cam(int u, int v);
|
||||||
|
std::vector<int> muban(Mat &left_image, Mat &right_image, const int *coordinates);
|
||||||
|
std::vector<Mat> get_range();
|
||||||
|
void horizon_estimate(Mat& img, Mat& bboxs,int k);
|
||||||
|
};
|
||||||
110
rknn_sdk.h
Normal file
110
rknn_sdk.h
Normal file
@ -0,0 +1,110 @@
|
|||||||
|
#ifndef __RKNN_SDK_H__
|
||||||
|
#define __RKNN_SDK_H__
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
// 返回错误码
|
||||||
|
enum XErrorCode {
|
||||||
|
RKNN_SUCCEED = 0,
|
||||||
|
RKNN_NOT_INIT = -1, //SDK未初始化
|
||||||
|
RKNN_INVAILD_IMGSIZE = -2, //输入图像尺寸不匹配模型输入要求
|
||||||
|
RKNN_INVAILD_PARAM = -3, //参数无效
|
||||||
|
};
|
||||||
|
|
||||||
|
enum ObjCls {
|
||||||
|
OBJ_Unknown = -1, //未知
|
||||||
|
OBJ_MaoGouWan = 0, //猫碗/狗碗
|
||||||
|
OBJ_BTYDiZuo = 1, //吧台椅底座
|
||||||
|
OBJ_FSDiZuo = 2, //风扇底座
|
||||||
|
OBJ_XiYiJi = 3, //洗衣机
|
||||||
|
OBJ_BingXiang = 4, //冰箱
|
||||||
|
OBJ_MaTong = 5, //马桶
|
||||||
|
OBJ_TiZhongCheng = 6, //体重秤
|
||||||
|
OBJ_DianXian = 7, //电线
|
||||||
|
OBJ_DianShiJi = 8, //电视机
|
||||||
|
OBJ_CanZhuo = 9, //餐桌
|
||||||
|
OBJ_DiTan = 10, //地毯
|
||||||
|
OBJ_MaBu = 11, //抹布
|
||||||
|
OBJ_ChaJi = 12, //茶几
|
||||||
|
OBJ_DianShiGui = 13, //电视柜
|
||||||
|
OBJ_XieZi = 14, //拖鞋/鞋子
|
||||||
|
OBJ_WaZi = 15, //袜子
|
||||||
|
OBJ_YiGui = 16, //衣柜
|
||||||
|
OBJ_Chuang = 17, //床
|
||||||
|
OBJ_ShaFa = 18, //沙发
|
||||||
|
OBJ_YiZi = 19, //椅子
|
||||||
|
OBJ_Men = 20, //门
|
||||||
|
OBJ_DLS = 21, //da li shi
|
||||||
|
OBJ_DiBan = 22 //di ban
|
||||||
|
//OBJ_Xiang = 21 //宠物粪便
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// 图像信息与buffer
|
||||||
|
typedef struct _InputData {
|
||||||
|
int shape[4]; // Num, Channel, Height, Width
|
||||||
|
void* data; // img.data
|
||||||
|
} InputData;
|
||||||
|
|
||||||
|
// 检测结果
|
||||||
|
typedef struct _DetRet
|
||||||
|
{
|
||||||
|
int nID; //图像ID
|
||||||
|
int nLabel; //物体类别 ObjCls nLabel; //物体类别
|
||||||
|
float fConf; //置信度
|
||||||
|
float fLeft; //检测框左上角比例坐标x_min -> int(x_min*width) 获取像素坐标
|
||||||
|
float fTop; //检测框左上角比例坐标y_min -> int(y_min*height) 获取像素坐标
|
||||||
|
float fRight; //检测框右下角比例坐标x_max -> int(x_max*width) 获取像素坐标
|
||||||
|
float fBottom; //检测框右下角比例坐标y_max -> int(y_max*height) 获取像素坐标
|
||||||
|
} DetRet;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#define RKNNSDK_API(retType) retType
|
||||||
|
|
||||||
|
///
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
typedef uint64_t rknn_handle;
|
||||||
|
|
||||||
|
|
||||||
|
/* RKNN_Create
|
||||||
|
SDK加载模型，仅初始化一次;
|
||||||
|
input:
|
||||||
|
rknn_handle hd 句柄
|
||||||
|
const char* model_path 模型路径
|
||||||
|
return:
|
||||||
|
int 错误码
|
||||||
|
*/
|
||||||
|
RKNNSDK_API(int) RKNN_Create(rknn_handle* hd, const char* model_path);
|
||||||
|
|
||||||
|
/* RKNN_ObjDet
|
||||||
|
输入图像，获取检测结果和检测目标数量;
|
||||||
|
input:
|
||||||
|
rknn_handle hd 句柄
|
||||||
|
const InputData* input_data 输入图像数据
|
||||||
|
DetRet** det_ret 获取检测结果
|
||||||
|
int* det_num 获取检测目标数量
|
||||||
|
return:
|
||||||
|
int 错误码
|
||||||
|
*/
|
||||||
|
RKNNSDK_API(int) RKNN_ObjDet(rknn_handle hd, const InputData* input_data, DetRet** det_ret, int* det_num);
|
||||||
|
|
||||||
|
/* RKNN_Release
|
||||||
|
SDK资源释放，仅在程序退出前调用;
|
||||||
|
input:
|
||||||
|
rknn_handle hd 句柄
|
||||||
|
return:
|
||||||
|
int 错误码
|
||||||
|
*/
|
||||||
|
RKNNSDK_API(int) RKNN_Release(rknn_handle hd);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif //__RKNN_SDK_H__
|
||||||
1
zhixing-1
Normal file
1
zhixing-1
Normal file
@ -0,0 +1 @@
|
|||||||
|
g++ -std=c++11 ./*.cpp -o main.out -I /usr/local/include -I /usr/local/include/opencv4 -I /usr/include -I /opt/ros/melodic/include -I /home/firefly/Desktop/object_detect_sdk_1804/install/include -L /usr/local/lib -l opencv_core -l opencv_imgproc -l opencv_imgcodecs -l opencv_highgui -l opencv_dnn -l opencv_videoio -l opencv_calib3d -L /usr/lib/obj_sdk -l rknn_sdk -l rknnrt -l roscpp -l roslib -l rosconsole -l roscpp_serialization -L /opt/ros/melodic/lib -l rostime -l boost_system -l pthread -l udev
|
||||||
Loading…
x
Reference in New Issue
Block a user