update pub_msg; limit filtering non-stones objs

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-03-28 06:58:22
+ * @LastEditTime: 2025-03-28 09:27:22
  */
 
 
@@ -160,6 +160,8 @@ void sensorManager::publishDetection()
             usleep(5000); // 5ms
             continue;
         }
+        std::cout << "Infer objects: " << frame_ptr->objs_.size() << std::endl;
+
 
         auto vGrid = std::make_shared<BEV_GridMap>();
         auto point_cloud_ptr = getCurPointcloud();
@@ -174,9 +176,9 @@ void sensorManager::publishDetection()
             vision_converter_->Process_objects(frame_ptr->img_src_, frame_ptr->depth_, frame_ptr->objs_, *vGrid);
         }
 
-        waytous_perception_msgs::ObjectArray obFrame;
-        obFrame.sensor_type = waytous_perception_msgs::Object::SENSOR_CAMERA;
+        std::cout << "BEV objects: " << vGrid->vInsObjs.size() << std::endl;
 
+        waytous_perception_msgs::ObjectArray obFrame;
         int8_t obj_id = 0;
         for(auto& obj: vGrid->vInsObjs)
         {
@@ -185,6 +187,7 @@ void sensorManager::publishDetection()
                 continue;
             }
             waytous_perception_msgs::Object ob;
+            ob.sensor_type = waytous_perception_msgs::Object::SENSOR_CAMERA;
 
             //int8 id = 1;   
             ob.id = obj_id++;
@@ -250,7 +253,7 @@ void sensorManager::publishDetection()
                 pxyzi.z = 0;
                 res_pcl_cloud->points.emplace_back(pxyzi);
             }
-            pcl::toROSMsg(*res_pcl_cloud, ob.cloud);
+            pcl::toROSMsg(*res_pcl_cloud, ob.pointcloud);
 
             // int8  label_type = 9
             ob.label_type = (name2type.find(obj.names) == name2type.end() ? Perception_Default_Classification : name2type[obj.names]);
@@ -279,7 +282,8 @@ void sensorManager::publishDetection()
             obFrame.foreground_objects.emplace_back(ob);
         }
         obj_publisher_.publish(obFrame);
-        std::cout<<" onRun :  "<<double(clock()-start_)/CLOCKS_PER_SEC<<"s"<<std::endl;  //输出时间（单位：ｓ）
+        std::cout<<" onRun :  "<<double(clock()-start_)/CLOCKS_PER_SEC<<"s.  "<< "Detected Objects: " 
+            << obFrame.foreground_objects.size() <<std::endl;  //输出时间（单位：ｓ）
 
         if(ros_visualization_ptr_ != nullptr && infer_param.vis_res){
             ros_visualization_ptr_->Visualization(*vGrid, frame_ptr->img_src_, frame_ptr->objs_);

src/camera_bev_infer/src/sensor_manager.h

@@ -4,8 +4,8 @@
  * @Author: wxin
  * @Date: 2023-09-03 01:53:20
  * @email: xin.wang@waytous.com
- * @LastEditors: xin.wang.waytous xin.wang@waytous.com
- * @LastEditTime: 2024-10-31 08:43:47
+ * @LastEditors: yangxue xue.yang@waytous.com
+ * @LastEditTime: 2025-03-28 08:31:29
  */
 
 #ifndef SENSORMANAGER_H
@@ -34,7 +34,7 @@
 // msg
 #include "waytous_perception_msgs/ObjectArray.h"
 #include "waytous_perception_msgs/Object.h"
-#include "waytous_perception_msgs/Rect.h"
+// #include "waytous_perception_msgs/Rect.h"
 
 #include "common_maps.h"
 #include "utils.h"

src/camera_bev_infer/src/vision_transform/vision_3d.cpp

@@ -4,8 +4,8 @@
  * @Author: wxin
  * @Date: 2023-09-03 03:13:41
  * @email: xin.wang@waytous.com
- * @LastEditors: wxin
- * @LastEditTime: 2024-03-12 06:38:25
+ * @LastEditors: yangxue xue.yang@waytous.com
+ * @LastEditTime: 2025-03-28 10:58:33
  */
 
 
@@ -401,6 +401,7 @@ int Visione_3D::Process_objects(const cv::Mat& image, const cv::Mat& imgdist, st
             Process_Segmantic_Objects(obj, imgdist, pTable, vGrid);
         }
     }
+    
     // 将静态目标投影到栅格地图中
     pcl::PointXYZI bev_point;
     cv::Point2f pt2f;
@@ -566,7 +567,11 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& di
             insobj.fromDepth = true;
             vGrid.vInsObjs.push_back(insobj);
             obj.valid = obj.valid + 2;
+        } else {
+            std::cout << "[Vision_3d.cpp] invalid object [rrc]: " << insobj.rrc.size.height << "  " << insobj.rrc.size.width << std::endl;
         }
+    } else {
+        std::cout << "[Vision_3d.cpp] invalid object [objRc]: " << nValidNum << " " << objRc.width << "   " << objRc.height << std::endl;
     }
     // std::cout<<" instance rc vision "<<double(clock()-start_)/CLOCKS_PER_SEC<<"s"<<std::endl;  //输出时间（单位：ｓ）
     return 0;
@@ -817,6 +822,7 @@ int Visione_3D::Process_objects(const cv::Mat& image, Cloud::Ptr& pcl_cloud, std
             Process_Segmantic_Objects(obj, point_index_in_camera, pcl_cloud, vGrid);
         }
     }
+
     // 将静态目标投影到栅格地图中
     pcl::PointXYZI bev_point;
     cv::Point2f pt2f;
@@ -966,12 +972,14 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
         Tracks[obj.track_id] = STrack(); // init
     }
     bool valid = Tracks[obj.track_id].valid;
-    // std::cout<<"obj: "<< obj.track_id<< ","<< obj.name <<" with project ratio: "<< project_ratio << ", " << project_count << std::endl;
+    std::cout<<"obj: "<< obj.track_id<< ","<< obj.name <<" with project ratio: "<< project_ratio << ", " << project_count << std::endl;
+    if(valid) std::cout << "valid----------------" << std::endl;
     if(valid){
         Tracks[obj.track_id].last_update_age = 0; // update
     }
     bool pointEnough = false;
     cv::Rect objRc = cv::Rect(objX1, objY1, MIN(objBin.cols, objX2 + 1) - objX1,  MIN(objBin.rows, objY2 + 1) - objY1);
+    
     //目标bev矩形区域
     InstanceBevObject insobj;
     if(nValidNum >= 3 && objRc.width > 0 && objRc.height > 0)
@@ -982,6 +990,8 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
             pointEnough = true;
         }
     }
+    if(pointEnough) std::cout << "point enough----------------" << std::endl;
+
     if(valid){
         if(pointEnough)
         {
@@ -993,7 +1003,7 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
             obj.valid = obj.valid + 1;
             // std::cout<< obj.track_id<<"valid----------------"<<std::endl;
         }else{
-            // std::cout<< obj.track_id<<"depth----------------"<<std::endl;
+            std::cout<< obj.track_id<<"not enough----------------"<<std::endl;
             return -1; // depth estimination
         }
     }else{
@@ -1014,6 +1024,7 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
                 }
             }
             // 获得高低差
+            std::cout << "obj points size: " << points3d.size() << std::endl;
             float min_z = 1e5, max_z = -1e5;
             for(auto& p3d_: points3d){
                 if(p3d_.z > max_z){
@@ -1023,8 +1034,9 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
                     min_z = p3d_.z;
                 }
             }
-
+            std::cout << "object height: " << max_z - min_z << "m, max-z: " << max_z << "m." << std::endl;
             bool filtered = (max_z - min_z < obstacle_height_threshold * 0.01) || (max_z < obstacle_height_threshold * 0.01 * 0.5);
+            if(obj.name != "stone") filtered = false; // dont filter pedestrian and vehicle
             if(!filtered){
                 Tracks[obj.track_id].valid_frame_num++; // add
                 std::cout << "------------------------ "<< obj.track_id <<": " << obj.name <<" with "<<points3d.size() << 
@@ -1038,6 +1050,8 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
                     vGrid.vInsObjs.push_back(insobj);
                     obj.valid = obj.valid + 1;
                 }
+            } else {
+                std::cout << "filtered!-------------------------" << std::endl;
             }
             // point enough, filtered by 
         }
@@ -1086,13 +1100,14 @@ int Visione_3D::Process_objects(const cv::Mat& image, const cv::Mat& depth, Clou
                     point_index_in_camera.at<cv::Vec2i>(y, x)[1] = i;
                 }
                 if(vis_project && i % sep == 0){
-                    auto color = indexs[0] == 0 ? cv::Scalar(0, 0, 255) : cv::Scalar(255, 255, 0);
+                    auto color = indexs[0] == 0 ? cv::Scalar(0, 0, 100) : cv::Scalar(255, 255, 0);
                     cv::circle(cv_image, cv::Point(x, y), 1, color, -1);
                 }
             }
         }
     }
     std::cout<<" pcl2camera with "<<camera_cloud->size()<<" points: "<<double(clock()-start_)/CLOCKS_PER_SEC<<"s"<<std::endl;  //输出时间（单位：ｓ）
+    std::cout << "[Vision_3d.cpp] objs size = " << objs.size() << std::endl;
 
     //循环每个目标
     // start_ = clock();
@@ -1125,6 +1140,8 @@ int Visione_3D::Process_objects(const cv::Mat& image, const cv::Mat& depth, Clou
             t++;
         }
     }
+    std::cout << "[Vision_3d.cpp] Process_objects size = " << vGrid.vInsObjs.size() << std::endl;
+
 
     // 将静态目标投影到栅格地图中
     pcl::PointXYZI bev_point;