2022.6.26
hong
commit
da545e2009
|
|
@ -0,0 +1,36 @@
|
|||
# LCFusion
|
||||
|
||||
#### Description
|
||||
激光雷达信息和视觉信息融合
|
||||
|
||||
#### Software Architecture
|
||||
Software architecture description
|
||||
|
||||
#### Installation
|
||||
|
||||
1. xxxx
|
||||
2. xxxx
|
||||
3. xxxx
|
||||
|
||||
#### Instructions
|
||||
|
||||
1. xxxx
|
||||
2. xxxx
|
||||
3. xxxx
|
||||
|
||||
#### Contribution
|
||||
|
||||
1. Fork the repository
|
||||
2. Create Feat_xxx branch
|
||||
3. Commit your code
|
||||
4. Create Pull Request
|
||||
|
||||
|
||||
#### Gitee Feature
|
||||
|
||||
1. You can use Readme\_XXX.md to support different languages, such as Readme\_en.md, Readme\_zh.md
|
||||
2. Gitee blog [blog.gitee.com](https://blog.gitee.com)
|
||||
3. Explore open source project [https://gitee.com/explore](https://gitee.com/explore)
|
||||
4. The most valuable open source project [GVP](https://gitee.com/gvp)
|
||||
5. The manual of Gitee [https://gitee.com/help](https://gitee.com/help)
|
||||
6. The most popular members [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/)
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
# LCFusion
|
||||
|
||||
#### 介绍
|
||||
激光雷达信息和视觉信息融合
|
||||
|
||||
#### 软件架构
|
||||
软件架构说明
|
||||
|
||||
|
||||
#### 安装教程
|
||||
|
||||
1. xxxx
|
||||
2. xxxx
|
||||
3. xxxx
|
||||
|
||||
#### 使用说明
|
||||
|
||||
1. xxxx
|
||||
2. xxxx
|
||||
3. xxxx
|
||||
|
||||
#### 参与贡献
|
||||
|
||||
1. Fork 本仓库
|
||||
2. 新建 Feat_xxx 分支
|
||||
3. 提交代码
|
||||
4. 新建 Pull Request
|
||||
|
||||
|
||||
#### 特技
|
||||
|
||||
1. 使用 Readme\_XXX.md 来支持不同的语言，例如 Readme\_en.md, Readme\_zh.md
|
||||
2. Gitee 官方博客 [blog.gitee.com](https://blog.gitee.com)
|
||||
3. 你可以 [https://gitee.com/explore](https://gitee.com/explore) 这个地址来了解 Gitee 上的优秀开源项目
|
||||
4. [GVP](https://gitee.com/gvp) 全称是 Gitee 最有价值开源项目，是综合评定出的优秀开源项目
|
||||
5. Gitee 官方提供的使用手册 [https://gitee.com/help](https://gitee.com/help)
|
||||
6. Gitee 封面人物是一档用来展示 Gitee 会员风采的栏目 [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/)
|
||||
|
|
@ -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.;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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}};
|
||||
}; // 类别映射，方便大小物体判断
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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 ********/
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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};
|
||||
}
|
||||
|
|
@ -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);
|
||||
};
|
||||
|
|
@ -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__
|
||||
|
|
@ -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…
Reference in New Issue