|
|
@@ -16,6 +16,10 @@
|
|
|
#include <thread>
|
|
|
#include "objectarray.pb.h"
|
|
|
//#include "ivbacktrace.h"
|
|
|
+#include "Tracking.hpp"
|
|
|
+
|
|
|
+#include "roiaware_pool3d.h"
|
|
|
+#include "Eigen/Dense"
|
|
|
|
|
|
iv::Ivfault *gfault = nullptr;
|
|
|
iv::Ivlog *givlog = nullptr;
|
|
|
@@ -29,33 +33,310 @@ const int kNumPointFeature = 5;
|
|
|
const int kOutputNumBoxFeature = 7;
|
|
|
std::string gstrinput;
|
|
|
std::string gstroutput;
|
|
|
+Eigen::Matrix3d rotation_matrix;
|
|
|
+Eigen::Vector3d trans_matrix;
|
|
|
+TrackerSettings settings;
|
|
|
+CTracker tracker(settings);
|
|
|
+bool m_isTrackerInitialized = false;
|
|
|
+
|
|
|
+
|
|
|
+#include <random>
|
|
|
+////////////////////用于nms////////////////////
|
|
|
+#include<opencv2/opencv.hpp>
|
|
|
+#define rad2Angle(rad) ((rad) * 180.0 / M_PI)
|
|
|
+
|
|
|
+const float smallinbig_threshold = 0.8;
|
|
|
+const float distance_threshold = 0.1;
|
|
|
+const float secondnms_threshold = 0.1;
|
|
|
+
|
|
|
+typedef struct {
|
|
|
+ cv::RotatedRect box;
|
|
|
+ std::vector<float> detection;
|
|
|
+ std::vector<float> bbox_pts;
|
|
|
+ int label;
|
|
|
+ float score;
|
|
|
+}BBOX3D;
|
|
|
+
|
|
|
+//将检测结果转为RotatedRect以便于nms计算
|
|
|
+bool GetRotatedRect(std::vector<float> &out_detections,std::vector<int> &out_labels,
|
|
|
+ std::vector<float> &out_scores, std::vector<BBOX3D> &results)
|
|
|
+{
|
|
|
+ int obj_size = out_detections.size()/kOutputNumBoxFeature;
|
|
|
+ if(out_labels.size()==obj_size && out_scores.size()==obj_size)
|
|
|
+ {
|
|
|
+ for(int i=0;i<obj_size;i++)
|
|
|
+ {
|
|
|
+ BBOX3D result;
|
|
|
+ result.box = cv::RotatedRect(cv::Point2f(out_detections.at(i*7),out_detections.at(i*7+1)),
|
|
|
+ cv::Size2f(out_detections.at(i*7+3),out_detections.at(i*7+4)),
|
|
|
+ rad2Angle(out_detections.at(i*7+6)));
|
|
|
+ for(int j=0;j<kOutputNumBoxFeature;j++)
|
|
|
+ result.detection.push_back(out_detections.at(i*7+j));
|
|
|
+ result.label = out_labels.at(i);
|
|
|
+ result.score = out_scores.at(i);
|
|
|
+ results.push_back(result);
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout<<"the size of detections labels scores is not equal !!!"<<std::endl;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+bool sort_score(BBOX3D box1,BBOX3D box2)
|
|
|
+{
|
|
|
+ return (box1.score > box2.score);
|
|
|
+}
|
|
|
+
|
|
|
+//计算两个旋转矩形的IOU
|
|
|
+float calcIOU(cv::RotatedRect rect1, cv::RotatedRect rect2)
|
|
|
+{
|
|
|
+ float areaRect1 = rect1.size.width * rect1.size.height;
|
|
|
+ float areaRect2 = rect2.size.width * rect2.size.height;
|
|
|
+ vector<cv::Point2f> vertices;
|
|
|
+ int intersectionType = cv::rotatedRectangleIntersection(rect1, rect2, vertices);
|
|
|
+ if (vertices.size()==0)
|
|
|
+ return 0.0;
|
|
|
+ else{
|
|
|
+ vector<cv::Point2f> order_pts;
|
|
|
+ cv::convexHull(cv::Mat(vertices), order_pts, true);
|
|
|
+ double area = cv::contourArea(order_pts);
|
|
|
+ float inner = (float) (area / (areaRect1 + areaRect2 - area + 0.0001));
|
|
|
+ //排除小框完全在大框里面的case
|
|
|
+ float areaMin = (areaRect1 < areaRect2)?areaRect1:areaRect2;
|
|
|
+ float innerMin = (float)(area / (areaMin + 0.0001));
|
|
|
+ if(innerMin > smallinbig_threshold)
|
|
|
+ inner = innerMin;
|
|
|
+ return inner;
|
|
|
+ }
|
|
|
+}
|
|
|
+//计算两个点的欧式距离
|
|
|
+float calcdistance(cv::Point2f center1, cv::Point2f center2)
|
|
|
+{
|
|
|
+ float distance = sqrt((center1.x-center2.x)*(center1.x-center2.x)+
|
|
|
+ (center1.y-center2.y)*(center1.y-center2.y));
|
|
|
+ return distance;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//nms
|
|
|
+void nms(std::vector<BBOX3D> &vec_boxs,float threshold,std::vector<BBOX3D> &results)
|
|
|
+{
|
|
|
+ std::sort(vec_boxs.begin(),vec_boxs.end(),sort_score);
|
|
|
+ while(vec_boxs.size() > 0)
|
|
|
+ {
|
|
|
+ results.push_back(vec_boxs[0]);
|
|
|
+ vec_boxs.erase(vec_boxs.begin());
|
|
|
+ for (auto it = vec_boxs.begin(); it != vec_boxs.end();)
|
|
|
+ {
|
|
|
+ float iou_value =calcIOU(results.back().box,(*it).box);
|
|
|
+ float distance_value = calcdistance(results.back().box.center,(*it).box.center);
|
|
|
+ if ((iou_value > threshold) || (distance_value<distance_threshold))
|
|
|
+ it = vec_boxs.erase(it);
|
|
|
+ else it++;
|
|
|
+ }
|
|
|
+
|
|
|
+// std::cout<<"results: "<<results.back().detection.at(0)<<" "<<results.back().detection.at(1)<<
|
|
|
+// " "<<results.back().detection.at(2)<<std::endl;
|
|
|
+
|
|
|
+ }
|
|
|
+}
|
|
|
+void GetLidarObj(std::vector<BBOX3D> &results,iv::lidar::objectarray & lidarobjvec)
|
|
|
+{
|
|
|
+ int i;
|
|
|
+ int obj_size = results.size();
|
|
|
+ // givlog->verbose("OBJ","object size is %d",obj_size);;
|
|
|
+ for(i=0;i<obj_size;i++)
|
|
|
+ {
|
|
|
+ iv::lidar::lidarobject lidarobj;
|
|
|
+ if (results.at(i).score < 0.10) {
|
|
|
+ std::cout<<"///////: "<<results.at(i).score<<std::endl;
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //if (results.at(i).label == 5) continue;
|
|
|
+
|
|
|
+ vector<float>out_detection = results.at(i).detection;
|
|
|
+ lidarobj.set_tyaw(out_detection.at(6));
|
|
|
+ iv::lidar::PointXYZ centroid;
|
|
|
+ iv::lidar::PointXYZ * _centroid;
|
|
|
+ centroid.set_x(out_detection.at(0));
|
|
|
+ centroid.set_y(out_detection.at(1));
|
|
|
+ centroid.set_z(out_detection.at(2));
|
|
|
+ _centroid = lidarobj.mutable_centroid();
|
|
|
+ _centroid->CopyFrom(centroid);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ min_point;
|
|
|
+ iv::lidar::PointXYZ * _min_point;
|
|
|
+ min_point.set_x(0);
|
|
|
+ min_point.set_y(0);
|
|
|
+ min_point.set_z(0);
|
|
|
+ _min_point = lidarobj.mutable_min_point();
|
|
|
+ _min_point->CopyFrom(min_point);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ max_point;
|
|
|
+ iv::lidar::PointXYZ * _max_point;
|
|
|
+ max_point.set_x(0);
|
|
|
+ max_point.set_y(0);
|
|
|
+ max_point.set_z(0);
|
|
|
+ _max_point = lidarobj.mutable_max_point();
|
|
|
+ _max_point->CopyFrom(max_point);
|
|
|
+ iv::lidar::PointXYZ position;
|
|
|
+ iv::lidar::PointXYZ * _position;
|
|
|
+ position.set_x(out_detection.at(0));
|
|
|
+ position.set_y(out_detection.at(1));
|
|
|
+ position.set_z(out_detection.at(2));
|
|
|
+ _position = lidarobj.mutable_position();
|
|
|
+ _position->CopyFrom(position);
|
|
|
+ lidarobj.set_mntype(results.at(i).label);
|
|
|
+ // label 2 8
|
|
|
+ if(results.at(i).label==2){
|
|
|
+ lidarobj.set_mntype(8);
|
|
|
+ }else if(results.at(i).label==8){
|
|
|
+ lidarobj.set_mntype(2);
|
|
|
+ }
|
|
|
+ lidarobj.set_score(results.at(i).score);
|
|
|
+ lidarobj.add_type_probs(results.at(i).score);
|
|
|
+ iv::lidar::PointXYZI point_cloud;
|
|
|
+ iv::lidar::PointXYZI * _point_cloud;
|
|
|
+ point_cloud.set_x(out_detection.at(0));
|
|
|
+ point_cloud.set_y(out_detection.at(1));
|
|
|
+ point_cloud.set_z(out_detection.at(2));
|
|
|
+ point_cloud.set_i(results.at(i).label);
|
|
|
+ _point_cloud = lidarobj.add_cloud();
|
|
|
+ _point_cloud->CopyFrom(point_cloud);
|
|
|
+ iv::lidar::Dimension ld;
|
|
|
+ iv::lidar::Dimension * pld;
|
|
|
+ ld.set_x(out_detection.at(3));
|
|
|
+ ld.set_y(out_detection.at(4));
|
|
|
+ ld.set_z(out_detection.at(5));
|
|
|
+ pld = lidarobj.mutable_dimensions();
|
|
|
+ pld->CopyFrom(ld);
|
|
|
+
|
|
|
+// std::cout<<"x y z: "<<out_detection.at(0)<<" "<<out_detection.at(1)<<" "<<
|
|
|
+// out_detection.at(2)<<" "<<out_detection.at(3)<<" "<< out_detection.at(4)<<" "
|
|
|
+// <<out_detection.at(5)<<" "<<out_detection.at(6)<<std::endl;
|
|
|
+
|
|
|
+ iv::lidar::lidarobject * po = lidarobjvec.add_obj();
|
|
|
+ po->CopyFrom(lidarobj);
|
|
|
+ }
|
|
|
+
|
|
|
+// std::cout<<"the lidarobjvec: "<<lidarobjvec.obj_size()<<std::endl;
|
|
|
+}
|
|
|
+////////////////////用于nms////////////////////
|
|
|
+
|
|
|
+
|
|
|
+////////////////////用于获得3dbbox中点云个数////////////////////
|
|
|
+#if 0
|
|
|
+inline void lidar_to_local_coords_cpu(float shift_x, float shift_y, float rot_angle, float &local_x, float &local_y){
|
|
|
+ float cosa = cos(-rot_angle), sina = sin(-rot_angle);
|
|
|
+ local_x = shift_x * cosa + shift_y * (-sina);
|
|
|
+ local_y = shift_x * sina + shift_y * cosa;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+inline int check_pt_in_box3d_cpu(const float *pt, std::vector<float> &box3d, float &local_x, float &local_y){
|
|
|
+ // param pt: (x, y, z)
|
|
|
+ // param box3d: [x, y, z, dx, dy, dz, heading], (x, y, z) is the box center
|
|
|
+ const float MARGIN = 1e-2;
|
|
|
+ float x = pt[0], y = pt[1], z = pt[2];
|
|
|
+ float cx = box3d[0], cy = box3d[1], cz = box3d[2];
|
|
|
+ float dx = box3d[3], dy = box3d[4], dz = box3d[5], rz = box3d[6];
|
|
|
+
|
|
|
+ if (fabsf(z - cz) > dz / 2.0) return 0;
|
|
|
+ lidar_to_local_coords_cpu(x - cx, y - cy, rz, local_x, local_y);
|
|
|
+ float in_flag = (fabs(local_x) < dx / 2.0 + MARGIN) & (fabs(local_y) < dy / 2.0 + MARGIN);
|
|
|
+ return in_flag;
|
|
|
+}
|
|
|
+
|
|
|
+int points_in_boxes_cpu(std::vector<BBOX3D>& boxes, float* pts_lidar,
|
|
|
+ int pts_num, std::vector<std::vector<int>>& pts_indices){
|
|
|
+
|
|
|
+ std::vector<std::vector<float>> pts_bboxes;
|
|
|
+
|
|
|
+ int boxes_num = boxes.size();
|
|
|
+ float local_x = 0, local_y = 0;
|
|
|
+ for (int i = 0; i < boxes_num; i++){
|
|
|
+ std::vector<float>pts_bbox;
|
|
|
+ for (int j = 0; j < pts_num; j++){
|
|
|
+ int cur_in_flag = check_pt_in_box3d_cpu(pts_lidar + j * 5, boxes[i].detection, local_x, local_y);
|
|
|
+ pts_indices[i][j] = cur_in_flag;
|
|
|
+ if(cur_in_flag)
|
|
|
+ {
|
|
|
+ pts_bbox.push_back(pts_lidar[j*5+0]);
|
|
|
+ pts_bbox.push_back(pts_lidar[j*5+1]);
|
|
|
+ pts_bbox.push_back(pts_lidar[j*5+2]);
|
|
|
+ pts_bbox.push_back(pts_lidar[j*5+3]);
|
|
|
+ pts_bbox.push_back(pts_lidar[j*5+4]);
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ pts_bboxes.push_back(pts_bbox);
|
|
|
+
|
|
|
+ std::cout<<"the size of points: "<<i<<" : "<<pts_bbox.size() / 5 <<std::endl;
|
|
|
+
|
|
|
+ pts_bbox.clear();
|
|
|
+ }
|
|
|
+
|
|
|
+ return 1;
|
|
|
+}
|
|
|
+
|
|
|
+#endif
|
|
|
+////////////////////用于获得3dbbox中点云个数////////////////////
|
|
|
|
|
|
|
|
|
void PclToArray(
|
|
|
- const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
|
|
|
- float* out_points_array, const float normalizing_factor) {
|
|
|
- for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
|
|
|
- pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
|
|
|
- out_points_array[i * 4 + 0] = point.x;
|
|
|
- out_points_array[i * 4 + 1] = point.y;
|
|
|
- out_points_array[i * 4 + 2] = point.z;
|
|
|
- out_points_array[i * 4 + 3] =
|
|
|
- static_cast<float>(point.intensity / normalizing_factor);
|
|
|
- }
|
|
|
+ const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
|
|
|
+ float* out_points_array, const float normalizing_factor) {
|
|
|
+ for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
|
|
|
+ pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
|
|
|
+ out_points_array[i * 4 + 0] = point.x;
|
|
|
+ out_points_array[i * 4 + 1] = point.y;
|
|
|
+ out_points_array[i * 4 + 2] = point.z;
|
|
|
+ out_points_array[i * 4 + 3] =
|
|
|
+ static_cast<float>(point.intensity / normalizing_factor);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
void PclXYZITToArray(
|
|
|
- const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
|
|
|
- float* out_points_array, const float normalizing_factor) {
|
|
|
- for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
|
|
|
- pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
|
|
|
- out_points_array[i * 5 + 0] = point.x;
|
|
|
- out_points_array[i * 5 + 1] = point.y;
|
|
|
- out_points_array[i * 5 + 2] = point.z;
|
|
|
- out_points_array[i * 5 + 3] =
|
|
|
- static_cast<float>(point.intensity / normalizing_factor);
|
|
|
- out_points_array[i * 5 + 4] = 0;
|
|
|
- }
|
|
|
+ const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
|
|
|
+ float* out_points_array, const float normalizing_factor) {
|
|
|
+
|
|
|
+ /////shuffle the index array/////
|
|
|
+ bool shuffle = true;
|
|
|
+ int point_num = in_pcl_pc_ptr->size();
|
|
|
+ std::vector<int>indices(point_num);
|
|
|
+ std::iota(indices.begin(),indices.end(),0);
|
|
|
+ if(shuffle)
|
|
|
+ {
|
|
|
+// unsigned seed = 0;
|
|
|
+// std::shuffle(indices.begin(),indices.end(),std::default_random_engine(seed));
|
|
|
+
|
|
|
+ std::random_device rd;
|
|
|
+ std::mt19937 g(rd());
|
|
|
+ std::shuffle(indices.begin(),indices.end(),g);
|
|
|
+
|
|
|
+ }
|
|
|
+ /////shuffle the index array/////
|
|
|
+
|
|
|
+ for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
|
|
|
+ //pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
|
|
|
+ int indice = indices[i];
|
|
|
+ pcl::PointXYZI point = in_pcl_pc_ptr->at(indice);
|
|
|
+ Eigen::Vector3d new_point, old_point;
|
|
|
+ old_point<<point.x, point.y, point.z;
|
|
|
+ new_point = rotation_matrix * (old_point) + trans_matrix;
|
|
|
+ out_points_array[i * 5 + 0] = new_point[0];
|
|
|
+ out_points_array[i * 5 + 1] = new_point[1];
|
|
|
+ out_points_array[i * 5 + 2] = new_point[2];
|
|
|
+ out_points_array[i * 5 + 3] =
|
|
|
+ static_cast<float>(point.intensity / normalizing_factor);
|
|
|
+ out_points_array[i * 5 + 4] = 0;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
|
|
|
@@ -67,8 +348,7 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
|
|
|
for(i=0;i<obj_size;i++)
|
|
|
{
|
|
|
iv::lidar::lidarobject lidarobj;
|
|
|
- if (out_scores.at(i) < 0.12) continue;
|
|
|
- if (out_labels.at(i) == 5) continue;
|
|
|
+ if (out_scores.at(i) < 0.10) continue;
|
|
|
|
|
|
lidarobj.set_tyaw(out_detections.at(i*7+6));
|
|
|
iv::lidar::PointXYZ centroid;
|
|
|
@@ -117,16 +397,16 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
|
|
|
point_cloud.set_x(out_detections.at(i*7));
|
|
|
point_cloud.set_y(out_detections.at(i*7+1));
|
|
|
point_cloud.set_z(out_detections.at(i*7+2));
|
|
|
- point_cloud.set_i(out_detections.at(out_labels.at(i)));
|
|
|
+ point_cloud.set_i(0);
|
|
|
|
|
|
_point_cloud = lidarobj.add_cloud();
|
|
|
_point_cloud->CopyFrom(point_cloud);
|
|
|
|
|
|
iv::lidar::Dimension ld;
|
|
|
iv::lidar::Dimension * pld;
|
|
|
- ld.set_x(out_detections.at(i*7+3));
|
|
|
- ld.set_y(out_detections.at(i*7+4));
|
|
|
- ld.set_z(out_detections.at(i*7+5));
|
|
|
+ ld.set_x(out_detections.at(i*7+3));// w
|
|
|
+ ld.set_y(out_detections.at(i*7+4));// l
|
|
|
+ ld.set_z(out_detections.at(i*7+5));// h
|
|
|
pld = lidarobj.mutable_dimensions();
|
|
|
pld->CopyFrom(ld);
|
|
|
|
|
|
@@ -134,15 +414,16 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
|
|
|
iv::lidar::lidarobject * po = lidarobjvec.add_obj();
|
|
|
po->CopyFrom(lidarobj);
|
|
|
}
|
|
|
+
|
|
|
}
|
|
|
|
|
|
void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
|
|
|
{
|
|
|
std::shared_ptr<float> points_array_ptr = std::shared_ptr<float>(new float[pc_ptr->size() * kNumPointFeature]);
|
|
|
- // float* points_array = new float[pc_ptr->size() * kNumPointFeature];
|
|
|
+ // float* points_array = new float[pc_ptr->size() * kNumPointFeature];
|
|
|
PclXYZITToArray(pc_ptr, points_array_ptr.get(), 1.0);
|
|
|
|
|
|
- int in_num_points = pc_ptr->width;
|
|
|
+ int in_num_points = pc_ptr->width;
|
|
|
|
|
|
std::vector<float> out_detections;
|
|
|
std::vector<int> out_labels;
|
|
|
@@ -151,34 +432,146 @@ void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
|
|
|
QTime xTime;
|
|
|
|
|
|
xTime.start();
|
|
|
-
|
|
|
+ auto startTime = std::chrono::high_resolution_clock::now();
|
|
|
cudaDeviceSynchronize();
|
|
|
pPillars->DoInference(points_array_ptr.get(), in_num_points, &out_detections, &out_labels , &out_scores);
|
|
|
cudaDeviceSynchronize();
|
|
|
- int BoxFeature = 7;
|
|
|
- int num_objects = out_detections.size() / BoxFeature;
|
|
|
+ auto endTime = std::chrono::high_resolution_clock::now();
|
|
|
+ double inferenceDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
|
|
|
+// std::cout<< "inferenceDuration Time: " << inferenceDuration << " ms"<< std::endl;
|
|
|
|
|
|
+
|
|
|
+// int BoxFeature = 7;
|
|
|
+// int num_objects = out_detections.size() / BoxFeature;
|
|
|
// givlog->verbose("obj size is %d", num_objects);
|
|
|
// std::cout<<"obj size is "<<num_objects<<std::endl;
|
|
|
|
|
|
-// std::vector<iv::lidar::lidarobject> lidarobjvec;
|
|
|
+
|
|
|
+// iv::lidar::objectarray lidarobjvec;
|
|
|
+// GetLidarObj(out_detections,out_labels,out_scores,lidarobjvec);
|
|
|
+
|
|
|
+ //////////nms//////////
|
|
|
+ //startTime = std::chrono::high_resolution_clock::now();
|
|
|
+ std::vector<BBOX3D>results_rect;
|
|
|
+ GetRotatedRect(out_detections,out_labels,out_scores,results_rect);
|
|
|
+// std::cout<<"results_rect size: "<<results_rect.size()<<std::endl;
|
|
|
+
|
|
|
+ std::vector<BBOX3D>results_bbox;
|
|
|
+ nms(results_rect,secondnms_threshold,results_bbox);
|
|
|
+// std::cout<<"results_bbox size: "<<results_bbox.size()<<std::endl;
|
|
|
+
|
|
|
+ //get lidar points in 3Dbbox in cpu
|
|
|
+// startTime = std::chrono::high_resolution_clock::now();
|
|
|
+// //get lidar points in 3Dbbox
|
|
|
+// int boxes_num = results_bbox.size();
|
|
|
+// std::vector<std::vector<int>>pts_indices(boxes_num, vector<int>(in_num_points, 0));
|
|
|
+// points_in_boxes_cpu(results_bbox,points_array_ptr.get(),in_num_points,pts_indices);
|
|
|
+// endTime = std::chrono::high_resolution_clock::now();
|
|
|
+// double nmsDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
|
|
|
+// std::cout <<"3DBoxDuration Time : "<<nmsDuration<< endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //get lidar points in 3Dbbox in gpu
|
|
|
+ startTime = std::chrono::high_resolution_clock::now();
|
|
|
+ float* dev_points;
|
|
|
+ GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_points),
|
|
|
+ in_num_points * 5 * sizeof(float))); // in_num_points , 5
|
|
|
+ GPU_CHECK(cudaMemset(dev_points, 0, in_num_points * 5 * sizeof(float)));
|
|
|
+ GPU_CHECK(cudaMemcpy(dev_points, points_array_ptr.get(),
|
|
|
+ in_num_points * 5 * sizeof(float),
|
|
|
+ cudaMemcpyHostToDevice));
|
|
|
+ int* box_idx_of_points_gpu;
|
|
|
+ GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&box_idx_of_points_gpu),
|
|
|
+ in_num_points * sizeof(int))); // in_num_points , 5
|
|
|
+ GPU_CHECK(cudaMemset(box_idx_of_points_gpu, -1, in_num_points * sizeof(float)));
|
|
|
+
|
|
|
+ int boxes_num = results_bbox.size();
|
|
|
+ float *boxes_cpu = new float[boxes_num*7];
|
|
|
+ for(int i=0;i<boxes_num;i++)
|
|
|
+ {
|
|
|
+ for(int j=0;j<7;j++)
|
|
|
+ *(boxes_cpu + (i*7+j)) = results_bbox[i].detection[j];
|
|
|
+ }
|
|
|
+ float *boxes_gpu;
|
|
|
+ GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&boxes_gpu),
|
|
|
+ boxes_num * 7 * sizeof(float))); // in_num_points , 5
|
|
|
+ GPU_CHECK(cudaMemset(boxes_gpu, 0, boxes_num * 7 * sizeof(float)));
|
|
|
+ GPU_CHECK(cudaMemcpy(boxes_gpu, boxes_cpu,boxes_num * 7 * sizeof(float),
|
|
|
+ cudaMemcpyHostToDevice));
|
|
|
+ int batch_size = 1;
|
|
|
+ points_in_boxes_launcher(batch_size,boxes_num,in_num_points,boxes_gpu,dev_points,box_idx_of_points_gpu);
|
|
|
+ int* box_idx_of_points_cpu = new int[in_num_points]();
|
|
|
+ cudaMemcpy(box_idx_of_points_cpu, box_idx_of_points_gpu, in_num_points * sizeof(int), cudaMemcpyDeviceToHost);
|
|
|
+ //vector<int> box_idx_of_points(box_idx_of_points_cpu,box_idx_of_points_cpu+in_num_points);
|
|
|
+
|
|
|
+
|
|
|
+ //cv::Mat image=cv::Mat::zeros(1200,1200,CV_8UC3);
|
|
|
+ //存储bbox的点云
|
|
|
+ for(int i=0; i < in_num_points; i++)
|
|
|
+ {
|
|
|
+
|
|
|
+ for(int j=0; j<boxes_num; j++)
|
|
|
+ {
|
|
|
+ if (box_idx_of_points_cpu[i] == j)
|
|
|
+ {
|
|
|
+ for(int idx=0; idx<5; idx++)
|
|
|
+ results_bbox[j].bbox_pts.push_back(points_array_ptr.get()[i*5+idx]);
|
|
|
+// int x = int(points_array_ptr.get()[i*5]*10+600);
|
|
|
+// int y = int(points_array_ptr.get()[i*5+1]*10+600);
|
|
|
+// cv::circle(image,cv::Point(x,y),2,cv::Scalar(0,0,255));
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+// for(int j=0; j<boxes_num; j++)
|
|
|
+// {
|
|
|
+
|
|
|
+// std::cout<<"num points in bbox: "<<results_bbox[j].bbox_pts.size()/5<<std::endl;
|
|
|
+// }
|
|
|
+
|
|
|
+
|
|
|
+ endTime = std::chrono::high_resolution_clock::now();
|
|
|
+ double gpuDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
|
|
|
+// std::cout <<"3DBoxDuration_gpu Time : "<<gpuDuration<< endl;
|
|
|
+
|
|
|
+
|
|
|
+ //endTime = std::chrono::high_resolution_clock::now();
|
|
|
+ //double nmsDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
|
|
|
+ //std::cout <<"nmsDuration Time : "<<(double)(ends - start)/ CLOCKS_PER_SEC*1000 << endl;
|
|
|
+
|
|
|
iv::lidar::objectarray lidarobjvec;
|
|
|
- GetLidarObj(out_detections,out_labels,out_scores,lidarobjvec);
|
|
|
+ GetLidarObj(results_bbox,lidarobjvec);
|
|
|
+ //////////nms//////////
|
|
|
|
|
|
double timex = pc_ptr->header.stamp;
|
|
|
timex = timex/1000.0;
|
|
|
lidarobjvec.set_timestamp(pc_ptr->header.stamp);
|
|
|
|
|
|
+ //--------------------------------------------- init tracker -------------------------------------------------
|
|
|
+// if (!m_isTrackerInitialized)
|
|
|
+// {
|
|
|
+// m_isTrackerInitialized = InitTracker(tracker);
|
|
|
+// if (!m_isTrackerInitialized)
|
|
|
+// {
|
|
|
+// std::cerr << "Tracker initialize error!!!" << std::endl;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// iv::lidar::objectarray trackedobjvec = Tracking(lidarobjvec, tracker);
|
|
|
+
|
|
|
+ //-------------------------------------------- end tracking --------------------------------------------------
|
|
|
+
|
|
|
int ntlen;
|
|
|
std::string out = lidarobjvec.SerializeAsString();
|
|
|
+
|
|
|
iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
|
|
|
|
|
|
-// givlog->verbose("lenth is %d",out.length());
|
|
|
+ // givlog->verbose("lenth is %d",out.length());
|
|
|
}
|
|
|
|
|
|
void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
{
|
|
|
-// std::cout<<" is ok ------------ "<<std::endl;
|
|
|
+ // std::cout<<" is ok ------------ "<<std::endl;
|
|
|
if(nSize <=16)return;
|
|
|
unsigned int * pHeadSize = (unsigned int *)strdata;
|
|
|
if(*pHeadSize > nSize)
|
|
|
@@ -210,9 +603,9 @@ void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigne
|
|
|
{
|
|
|
pcl::PointXYZI xp;
|
|
|
memcpy(&xp,p,sizeof(pcl::PointXYZI));
|
|
|
- xp.z = xp.z;
|
|
|
+ xp.z = xp.z;p++;
|
|
|
+ if (((abs(xp.x)<0.5)&&(xp.y<0.5 && xp.y > -1.0)) || xp.z > 3.0 ) continue;
|
|
|
point_cloud->push_back(xp);
|
|
|
- p++;
|
|
|
}
|
|
|
|
|
|
DectectOnePCD(point_cloud);
|
|
|
@@ -272,10 +665,7 @@ void exitfunc()
|
|
|
iv::modulecomm::Unregister(gpdetect);
|
|
|
std::cout<<"exit func complete"<<std::endl;
|
|
|
}
|
|
|
-
|
|
|
-#include <QDir>
|
|
|
#include <QFile>
|
|
|
-
|
|
|
bool trtisexist(std::string strpfe,std::string strbackbone)
|
|
|
{
|
|
|
QFile xFile;
|
|
|
@@ -291,13 +681,12 @@ bool trtisexist(std::string strpfe,std::string strbackbone)
|
|
|
}
|
|
|
return true;
|
|
|
}
|
|
|
-
|
|
|
int main(int argc, char *argv[])
|
|
|
{
|
|
|
QCoreApplication a(argc, argv);
|
|
|
|
|
|
-// RegisterIVBackTrace();
|
|
|
-
|
|
|
+ // RegisterIVBackTrace();
|
|
|
+ tracker.setSettings(settings);
|
|
|
gfault = new iv::Ivfault("lidar_pointpillar");
|
|
|
givlog = new iv::Ivlog("lidar_pointpillar");
|
|
|
|
|
|
@@ -306,13 +695,11 @@ int main(int argc, char *argv[])
|
|
|
char * strhome = getenv("HOME");
|
|
|
std::string pfe_file = strhome;
|
|
|
pfe_file += "/models/lidar/cbgs_pp_multihead_pfe.onnx";
|
|
|
- std::string pfe_trt_file = strhome;
|
|
|
- pfe_trt_file += "/models/lidar/cbgs_pp_multihead_pfe.trt";
|
|
|
+ std::string pfe_trt_file = pfe_file.substr(0, pfe_file.find(".")) + ".trt";
|
|
|
|
|
|
std::string backbone_file = strhome;
|
|
|
backbone_file += "/models/lidar/cbgs_pp_multihead_backbone.onnx";
|
|
|
- std::string backbone_trt_file = strhome;
|
|
|
- backbone_trt_file += "/models/lidar/cbgs_pp_multihead_backbone.trt";
|
|
|
+ std::string backbone_trt_file = backbone_file.substr(0, backbone_file.find(".")) + ".trt";
|
|
|
|
|
|
bool btrtexist = trtisexist(pfe_trt_file,backbone_trt_file);
|
|
|
|
|
|
@@ -332,33 +719,40 @@ int main(int argc, char *argv[])
|
|
|
backbone_file = xparam.GetParam("backbone_file",backbone_file.data());
|
|
|
gstrinput = xparam.GetParam("input","lidar_pc");
|
|
|
gstroutput = xparam.GetParam("output","lidar_pointpillar");
|
|
|
+
|
|
|
if(btrtexist == false)
|
|
|
{
|
|
|
|
|
|
- std::cout<<"use onnx model."<<std::endl;
|
|
|
- pPillars = new PointPillars(
|
|
|
- 0.15,
|
|
|
- 0.10,
|
|
|
- true,
|
|
|
- pfe_file,
|
|
|
- backbone_file,
|
|
|
- pp_config
|
|
|
- );
|
|
|
+ std::cout<<"use onnx model."<<std::endl;
|
|
|
+ pPillars = new PointPillars(
|
|
|
+ 0.15,
|
|
|
+ 0.10,
|
|
|
+ true,
|
|
|
+ pfe_file,
|
|
|
+ backbone_file,
|
|
|
+ pp_config
|
|
|
+ );
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- std::cout<<"use engine mode."<<std::endl;
|
|
|
- pPillars = new PointPillars(
|
|
|
- 0.15,
|
|
|
- 0.10,
|
|
|
- false,
|
|
|
- pfe_trt_file,
|
|
|
- backbone_trt_file,
|
|
|
- pp_config
|
|
|
- );
|
|
|
+ std::cout<<"use trt model."<<std::endl;
|
|
|
+ pPillars = new PointPillars(
|
|
|
+ 0.15,
|
|
|
+ 0.10,
|
|
|
+ false,
|
|
|
+ pfe_trt_file,
|
|
|
+ backbone_trt_file,
|
|
|
+ pp_config
|
|
|
+ );
|
|
|
}
|
|
|
std::cout<<"PointPillars Init OK."<<std::endl;
|
|
|
-
|
|
|
+ Eigen::AngleAxisd r_z ( -0.0418, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw +
|
|
|
+ Eigen::AngleAxisd r_y ( -0.0242, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll +
|
|
|
+ Eigen::AngleAxisd r_x ( -0.0137, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
|
|
|
+ Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
|
|
|
+ // 四元数-->>旋转矩阵
|
|
|
+ rotation_matrix = q_zyx.toRotationMatrix();
|
|
|
+ trans_matrix << 0, 1.25, 0.72;
|
|
|
gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
|
|
|
gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
|
|
|
gpthread = new std::thread(statethread);
|
tianxiaosen