add calib and transform bbox3d

src/camera_bev_infer/configs/CameraBevParam.yaml

@@ -11,22 +11,32 @@ rgb_config_path: configs/weights/multi_yolov5m.yaml
 device: 0
 period_time: 5
 vis_res: true
-vis_frame_id: vehicle_link
+vis_frame_id: livox_frame_hap # vehicle_link
 vis_project: true
 # ["pedestrian", "vehicle", "stone"]
-pub_classes: ["pedestrian", "vehicle", "stone"] # ["stone", "cone"]
+pub_classes: ["pedestrian", "vehicle"] # ["stone", "cone"]
 obstacle_height_threshold: 25 # cm
-camera_intrinsic: [1942.60953875808,    0,                 966.598714709598,
-                   0,                   1941.99570266726,  574.677777251193,
+camera_intrinsic: [1018.532796,    0,                 1001.098828,
+                   0,                   1017.127611,  599.751335,
                    0,                   0,                 1 ]
-distortion_coefficients: [-0.528765703554843, 0.252351515780946, 0.00337392775054378, 0.000123062634511131, -0.0366309311422936]
+distortion_coefficients: [-0.385858, 0.125180, -0.004831, -0.001959, 0.000000]
 
-camera2lidar_extrinsic:  [  0.0071898, -0.9998431,  0.0161908, -0.050,
-                           -0.2606066, -0.0175052, -0.9652864,  2.225,
-                            0.9654183,  0.0027208, -0.2606915,  0.649,
-                            0,          0,          0,          1 ]
+camera2lidar_extrinsic:  [-0.05073, -0.993745, 0.0994831, -0.255167,
+                          -0.130468, -0.0921635, -0.98716, -0.0198193,
+                          0.990154, -0.063058, -0.124977, 0.76145,
+                          0,          0,          0,          1 ]
 src_img_width: 1920
 src_img_height: 1080
 dst_img_width: 960
 dst_img_height: 540
 
+# ji dong:
+# camera_intrinsic: [1942.60953875808,    0,                 966.598714709598,
+#                    0,                   1941.99570266726,  574.677777251193,
+#                    0,                   0,                 1 ]
+# distortion_coefficients: [-0.528765703554843, 0.252351515780946, 0.00337392775054378, 0.000123062634511131, -0.0366309311422936]
+
+# camera2lidar_extrinsic:  [  0.0071898, -0.9998431,  0.0161908, -0.050,
+#                            -0.2606066, -0.0175052, -0.9652864,  2.225,
+#                             0.9654183,  0.0027208, -0.2606915,  0.649,
+#                             0,          0,          0,          1 ]

src/camera_bev_infer/src/sensor_manager.cpp

@@ -5,7 +5,7 @@
  * @Date: 2023-09-03 02:18:07
  * @email: xin.wang@waytous.com
  * @LastEditors: yangxue xue.yang@waytous.com
- * @LastEditTime: 2025-04-08 03:22:23
+ * @LastEditTime: 2025-04-10 06:31:09
  */
 
 
@@ -185,7 +185,7 @@ void sensorManager::publishDetection()
         waytous_perception_msgs::ObjectArray obFrame;
         waytous_perception_msgs::ObjectTrackArray obTrackFrame;
         obTrackFrame.header = frame_ptr->sensor_header;
-        obTrackFrame.header.frame_id = infer_param.vis_frame_id;
+        obTrackFrame.header.frame_id = "vehicle_link";
         // obTrackFrame.header.stamp = ros::Time(frame_ptr->timestamp_);
 
         int8_t obj_id = 0;
@@ -230,8 +230,8 @@ void sensorManager::publishDetection()
             ::Eigen::Quaterniond orient;  
             orient = matR;
 
-            ob.pose.position.x = pos.x;
-            ob.pose.position.y = pos.y;
+            ob.pose.position.x = pos.x + lidar2vehicle_(0,3);
+            ob.pose.position.y = pos.y + lidar2vehicle_(1,3);
             ob.pose.position.z = 0;
             ob.pose.orientation.x = orient.x();
             ob.pose.orientation.y = orient.y();

src/camera_bev_infer/src/sensor_manager.h

@@ -5,7 +5,7 @@
  * @Date: 2023-09-03 01:53:20
  * @email: xin.wang@waytous.com
  * @LastEditors: yangxue xue.yang@waytous.com
- * @LastEditTime: 2025-04-07 09:02:10
+ * @LastEditTime: 2025-04-10 06:33:42
  */
 
 #ifndef SENSORMANAGER_H
@@ -151,22 +151,28 @@ public:
             pc_timestamp_sys_ = GetMillSecondTimeStamp();           
            
             ros::Time input_time = pdata->header.stamp;
-            std::string cloud_frame = pdata->header.frame_id;            
+            std::string cloud_frame = pdata->header.frame_id;
+            current_pointcloud_ = pdata;            
 
-            // 初始化 current_pointcloud_
-            current_pointcloud_ = boost::make_shared<sensor_msgs::PointCloud2>();
+            // // 初始化 current_pointcloud_
+            // current_pointcloud_ = boost::make_shared<sensor_msgs::PointCloud2>();
             
-            // std::cout << "get lidar2vehicle & transform pc to vehicle frame: base_frame:" << base_frame_ << ",  lidar frame:" << cloud_frame << std::endl;
+            // // std::cout << "get lidar2vehicle & transform pc to vehicle frame: base_frame:" << base_frame_ << ",  lidar frame:" << cloud_frame << std::endl;
             tran_->waitForTransform(base_frame_, cloud_frame, input_time, ros::Duration(10.0));  // destination_frame, original_frame, cant tf tree receive
-            tran_->lookupTransform(base_frame_, cloud_frame, ros::Time(0), lidar2vehicle_);            
-            pcl_ros::transformPointCloud(base_frame_, *pdata, *current_pointcloud_, *tran_);
+            tran_->lookupTransform(base_frame_, cloud_frame, ros::Time(0), lidar2vehicle_tf_);            
+            // pcl_ros::transformPointCloud(base_frame_, *pdata, *current_pointcloud_, *tran_);
+            // std::cout << "current_pointcloud_ header: " << current_pointcloud_->header.stamp << std::endl;
+            // std::cout << "current_pointcloud_ frame_id: " << current_pointcloud_->header.frame_id << std::endl;
             
             // std::cout << "update lidar2camera  with lidar2vehicle_: " << std::endl;
             // update lidar2camera  with lidar2vehicle_
-            Eigen::Matrix4d lidar2vehicle = transformTFToEigen(lidar2vehicle_);
-            Eigen::Matrix4d camera2lidar = infer_param.cameraParam_->camera2lidar_extrinsic;
-            Eigen::Matrix4d camera2lidar_new = lidar2vehicle * camera2lidar; // camera2vehicle actually!        
-            vision_converter_->setLidar2Camera(camera2lidar_new.inverse());
+            lidar2vehicle_ = transformTFToEigen(lidar2vehicle_tf_);
+
+            // Eigen::Matrix4d camera2lidar = infer_param.cameraParam_->camera2lidar_extrinsic;
+            // Eigen::Matrix4d camera2lidar_new = lidar2vehicle * camera2lidar; // camera2vehicle actually!
+            // std::cout << "camera2lidar: \n" << camera2lidar << std::endl;
+            // std::cout << "lidar2vehicle: \n" << lidar2vehicle_ << std::endl;
+            // vision_converter_->setLidar2Camera(camera2lidar_new.inverse());
         }
     };
 
@@ -216,8 +222,8 @@ private:
 
     tf::TransformListener *tran_;         // tf监听器
     std::string base_frame_ = "vehicle_link";              // 目标坐标系
-    tf::StampedTransform lidar2vehicle_; // lidar-vehicle transform
-    
+    tf::StampedTransform lidar2vehicle_tf_; // lidar-vehicle transform
+    Eigen::Matrix4d lidar2vehicle_;    
 
     ros::Publisher obj_publisher_; // 交通障碍物发布者
     ros::Subscriber image_subscriber_; // 图像订阅者
@@ -247,7 +253,7 @@ private:
 
     // 图像和点云数据指针
     sensor_msgs::ImageConstPtr current_image_ = nullptr;
-    sensor_msgs::PointCloud2Ptr current_pointcloud_ = nullptr;
+    sensor_msgs::PointCloud2ConstPtr current_pointcloud_ = nullptr;
     /// CloudConstPtr current_pointcloud_ = nullptr; // 点云数据指针
 
 };

src/camera_bev_infer/src/vision_transform/vision_3d.h

@@ -5,7 +5,7 @@
  * @Date: 2023-04-13 14:37:39
  * @emal: qingshui.cheng@waytous.com
  * @LastEditors: yangxue xue.yang@waytous.com
- * @LastEditTime: 2025-04-07 07:22:03
+ * @LastEditTime: 2025-04-10 07:21:11
  */
 
 #ifndef VISION_3D_H
@@ -189,6 +189,7 @@ public:
     void setLidar2Camera(const Eigen::Matrix4d& lidar2camera)
     {
         m_lidar2camera = lidar2camera;
+        // std::cout << "m_lidar2camera: \n" << m_lidar2camera << std::endl;
     }
 
 private: