Skip to content

R
ros_camera_bev
Commit dac2e427 authored by yangxue's avatar yangxue
Browse files
Options

update pub_msg; limit filtering non-stones objs

parent b88313e6
Show whitespace changes
Inline Side-by-side
Showing with 35 additions and 14 deletions
src/camera_bev_infer/src/sensor_manager.cpp
View file @ dac2e427
......@@ -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
View file @ dac2e427
......@@ -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
View file @ dac2e427
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment