change detction_lidar_centerpoint, have some problem. not complete.

src/detection/detection_lidar_centerpoint/main.cpp

@@ -6,16 +6,21 @@
 #include <lidar_centerpoint/centerpoint_trt.hpp>
 #include <lidar_centerpoint/preprocess/pointcloud_densification.hpp>
 
+#include "objectarray.pb.h"
+
 
 #include "modulecomm.h"
 
 void * gpa;
 void * gpdetect;
 
+
 using namespace centerpoint;
 
 std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
 
+std::vector<std::string> class_names_;
+
 void LoadYaml(std::string stryamlname,std::vector<int64_t> & allow_remapping_by_area_matrix
               )
 {
@@ -51,6 +56,15 @@ void init()
     std::vector<double> min_area_matrix ;
     std::vector<double> max_area_matrix ;
 
+    class_names_.push_back("CAR");
+    class_names_.push_back("TRUCK");
+    class_names_.push_back("BUS");
+    class_names_.push_back("BICYCLE");
+    class_names_.push_back("PEDESTRIAN");
+
+
+
+
 
     NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision);
     NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision);
@@ -68,6 +82,50 @@ void init()
 
 }
 
+
+void box3DToDetectedObject(
+  const Box3D & box3d, const std::vector<std::string> & class_names, const bool has_twist,
+  iv::lidar::lidarobject & obj)
+{
+  // TODO(yukke42): the value of classification confidence of DNN, not probability.
+//  obj.existence_probability = box3d.score;
+  obj.set_score(box3d.score);
+
+  // classification
+  if (box3d.label >= 0 && static_cast<size_t>(box3d.label) < class_names.size()) {
+    obj.set_type_name(class_names_[box3d.label]);
+  } else {
+    obj.set_type_name("UNKNOWN");
+  }
+
+
+  obj.set_mntype(box3d.label);
+  obj.set_tyaw(box3d.yaw);
+  iv::lidar::PointXYZ pos;
+  pos.set_x(box3d.x);pos.set_y(box3d.y);pos.set_z(box3d.z);
+  iv::lidar::Dimension dim;
+  dim.set_x(box3d.length);dim.set_y(box3d.width);dim.set_z(box3d.height);
+
+  iv::lidar::PointXYZ * ppos = obj.mutable_position();
+  ppos->CopyFrom(pos);
+  iv::lidar::PointXYZ * pcen = obj.mutable_centroid();
+  pcen->CopyFrom(pos);
+  iv::lidar::Dimension * pdim = obj.mutable_dimensions();
+  pdim->CopyFrom(dim);
+
+
+  // twist
+  if (has_twist) {
+//    float vel_x = box3d.vel_x;
+//    float vel_y = box3d.vel_y;
+//    geometry_msgs::msg::Twist twist;
+//    twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2));
+//    twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw);
+//    obj.kinematics.twist_with_covariance.twist = twist;
+//    obj.kinematics.has_twist = has_twist;
+  }
+}
+
 void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     if(nSize <=16)return;
@@ -123,6 +181,21 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
     detector_ptr_ ->detect(point_cloud,det_boxes3d);
     std::cout<<" box size: "<<det_boxes3d.size()<<std::endl;
 
+    iv::lidar::objectarray  lidarobjvec;
+
+    int nsize = static_cast<int>(det_boxes3d.size());
+    for(i=0;i<nsize;i++)
+    {
+        iv::lidar::lidarobject obj;
+        box3DToDetectedObject(det_boxes3d[i],class_names_,false,obj);
+        iv::lidar::lidarobject * pobj = lidarobjvec.add_obj();
+        pobj->CopyFrom(obj);
+    }
+
+    std::string out = lidarobjvec.SerializeAsString();
+    //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
+    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
+
 
 }
 
@@ -151,9 +224,10 @@ int main(int argc, char *argv[])
     init();
 
     std::string path = "/home/nvidia/1.pcd";
-    testdet(path);
+//    testdet(path);
 
-    gpa = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
+    gpa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);
+    gpdetect = iv::modulecomm::RegisterSend("lidar_track",10000000,1);
 
     return a.exec();
 }

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -78,6 +78,7 @@ INCLUDEPATH += $$PWD/../../include/msgtype
 
 INCLUDEPATH += /usr/include/opencv4/
 LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
+#LIBS += /usr/lib/libopencv*.so
 
 #LIBS += /usr/lib/x86_64-linux-gnu/libopencv*.so