Skip to content

R
ros_camera_bev
Commit 6150ca42 authored by xin.wang.waytous's avatar xin.wang.waytous
Browse files
Options

stone-tracker-filter

parent 67f18462
Hide whitespace changes
Inline Side-by-side
Showing with 157 additions and 110 deletions
src/camera_bev_infer/configs/CameraBevParam.yaml
View file @ 6150ca42
rgb_sub_topic_name: /front_rgb/image
rgb_sub_topic_name: /sensor/camera/front_middle/image/bgr
pointcloud_sub_topic_name: /total_calib_pointcloud
detection_pub_topic_name: /perception/camera/obstacle
# static_gridmap_pub_topic_name: /perception/camera/bev_static_gridmap
......@@ -12,21 +12,17 @@ device: 0
period_time: 5
vis_res: true
vis_frame_id: Lidar
vis_project: false
# ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
vis_project: true
# ["pedestrian", "vehicle", "stone"]
pub_classes: ["stone", "cone"]
obstacle_height_threshold: 25 # cm
obstacle_height_threshold: 5 # cm
camera_intrinsic: [1346.61241105747, 0, 951.014999441565,
0, 1344.43795511862, 530.562496955385,
0, 0, 1]
distortion_coefficients: [-0.449196898930979, 0.253956763950171, -0.00139403693782871, -0.00111424348026907, -0.0870171627456232]
# camera2lidar_extrinsic: [-0.0423904, -0.999009, 0.0135863, 0.0656235,
# 0.0313214, -0.0149206, -0.999398, -0.167858,
# 0.99861, -0.0419393, 0.0319228, -0.0162872,
# 0, 0, 0, 1]
camera2lidar_extrinsic: [ 0.0029218, -0.9999551, 0.0090201, -0.339,
0.0170034, -0.0089692, -0.9998152, 3.404,
0.9998512, 0.0030746, 0.0169764, -0.164,
camera2lidar_extrinsic: [ 0.0177667, -0.9994557, -0.0277972, 0.258,
-0.0891004, 0.0261083, -0.9956804, 3.431,
0.9958642, 0.0201667, -0.0885880, 0.212,
0, 0, 0, 1 ]
src_img_width: 1920
src_img_height: 1080
......
src/camera_bev_infer/configs/CameraBevParam_jd07.yaml 0 → 100644
View file @ 6150ca42
rgb_sub_topic_name: /sensor/camera/front_middle/image/bgr
pointcloud_sub_topic_name: /total_calib_pointcloud
detection_pub_topic_name: /camera/objects
# static_gridmap_pub_topic_name: /perception/camera/bev_static_gridmap
creator_id: 0000
creator_name: camera_bev_node
sensor_frame_id: camera_01
config_root_path: /home/nvidia/Projects/perception/ros_camera_bev/src/camera_bev_infer
#config_root_path: /home/ubuntu/projects/ros_camera_bev/src/camera_bev_infer
rgb_config_path: configs/weights/multi_yolov5m.yaml
device: 0
period_time: 5
vis_res: true
vis_frame_id: Lidar
vis_project: false
# ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
pub_classes: ["stone", "cone"]
obstacle_height_threshold: 25 # cm
camera_intrinsic: [1346.61241105747, 0, 951.014999441565,
0, 1344.43795511862, 530.562496955385,
0, 0, 1]
distortion_coefficients: [-0.449196898930979, 0.253956763950171, -0.00139403693782871, -0.00111424348026907, -0.0870171627456232]
camera2lidar_extrinsic: [ 0.0177667, -0.9994557, -0.0277972, 0.258,
-0.0891004, 0.0261083, -0.9956804, 3.431,
0.9958642, 0.0201667, -0.0885880, 0.212,
0, 0, 0, 1 ]
src_img_width: 1920
src_img_height: 1080
dst_img_width: 960
dst_img_height: 540
src/camera_bev_infer/configs/CameraBevParam_jd08.yaml 0 → 100644
View file @ 6150ca42
rgb_sub_topic_name: /sensor/camera/front_middle/image/bgr
pointcloud_sub_topic_name: /total_calib_pointcloud
detection_pub_topic_name: /camera/objects
# static_gridmap_pub_topic_name: /perception/camera/bev_static_gridmap
creator_id: 0000
creator_name: camera_bev_node
sensor_frame_id: camera_01
config_root_path: /home/nvidia/Projects/perception/ros_camera_bev/src/camera_bev_infer
#config_root_path: /home/ubuntu/projects/ros_camera_bev/src/camera_bev_infer
rgb_config_path: configs/weights/multi_yolov5m.yaml
device: 0
period_time: 5
vis_res: true
vis_frame_id: Lidar
vis_project: false
# ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
pub_classes: ["stone", "cone"]
obstacle_height_threshold: 25 # cm
camera_intrinsic: [1346.61241105747, 0, 951.014999441565,
0, 1344.43795511862, 530.562496955385,
0, 0, 1]
distortion_coefficients: [-0.449196898930979, 0.253956763950171, -0.00139403693782871, -0.00111424348026907, -0.0870171627456232]
camera2lidar_extrinsic: [ 0.0357085, -0.9993340, -0.0075053, 0.168,
-0.0840158, 0.0044816, -0.9964544, 3.492,
0.9958244, 0.0362125, -0.0837998, 0.074,
0, 0, 0, 1 ]
src_img_width: 1920
src_img_height: 1080
dst_img_width: 960
dst_img_height: 540
src/camera_bev_infer/configs/weights/multi_yolov5m.yaml
View file @ 6150ca42
......@@ -28,8 +28,8 @@ units:
inputNames: [resizeNormImages]
outputNames: [detections, seg_protos, depths, semantics]
runMode: 1 # 0-fp32 1-fp16 2-int8 (int8 not supported)
weightsPath: "configs/weights/isp_m_best.onnx"
engineFile: "configs/weights/isp_m_best_fp16.engine"
weightsPath: "configs/weights/isp_x0228m_last.onnx"
engineFile: "configs/weights/isp_x0228m_last_fp16.engine"
inferDynamic: false
inferBatchSize: 1
inputWidth: 960
......@@ -47,12 +47,12 @@ units:
scoreThreshold: 0.15 # used when inference, can be modified
truncatedThreshold: 0.05
maxOutputNum: 1000
rawDetectionLength: 32130 # 25200
rawDetectionLength: 42840 # 32130
keepTopK: 100
segProtoDim: 32
instanceDownScale: 4
instanceClassNumber: 9
instanceClassNames: ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
instanceClassNumber: 3
instanceClassNames: ["pedestrian", "vehicle", "stone"]
# semantic-seg
semanticDownScale: 4
semanticClassNumber: 2
......@@ -66,7 +66,7 @@ units:
outputNames: [tracked_instances]
frame_rate: 10
track_buffer: 30
track_thresh: 0.2
track_thresh: 0.15
high_thresh: 0.2
match_thresh: 0.6
init_frames: 2
......
src/camera_bev_infer/configs/weights/multi_yolov5s.yaml → src/camera_bev_infer/configs/weights/multi_yolov5m_v0.yaml
View file @ 6150ca42
name: CameraModel
inputNames: [cvImage]
outputNames: [out_instances, out_semantics, out_depths, undistortVisImage]
outputNames: [tracked_instances, out_semantics, out_depths, undistortVisImage]
units:
-
......@@ -28,8 +28,8 @@ units:
inputNames: [resizeNormImages]
outputNames: [detections, seg_protos, depths, semantics]
runMode: 1 # 0-fp32 1-fp16 2-int8 (int8 not supported)
weightsPath: "configs/weights/isp_s_best.onnx"
engineFile: "configs/weights/isp_s_best_fp16.engine"
weightsPath: "configs/weights/isp_m_best.onnx"
engineFile: "configs/weights/isp_m_best_fp16.engine"
inferDynamic: false
inferBatchSize: 1
inputWidth: 960
......@@ -44,7 +44,7 @@ units:
fixAspectRatio: false
# instance-seg
nmsThreshold: 0.45
scoreThreshold: 0.2 # used when inference, can be modified
scoreThreshold: 0.15 # used when inference, can be modified
truncatedThreshold: 0.05
maxOutputNum: 1000
rawDetectionLength: 32130 # 25200
......@@ -60,6 +60,17 @@ units:
# depth
depthDownScale: 4
depthDistanceScale: 256
-
name: ByteTracker
inputNames: [out_instances]
outputNames: [tracked_instances]
frame_rate: 10
track_buffer: 30
track_thresh: 0.15
high_thresh: 0.15
match_thresh: 0.6
init_frames: 2
out_lost_threshold: 0
......
src/camera_bev_infer/src/sensor_manager.h
View file @ 6150ca42
......@@ -5,7 +5,7 @@
* @Date: 2023-09-03 01:53:20
* @email: xin.wang@waytous.com
* @LastEditors: wxin
* @LastEditTime: 2023-12-14 07:06:26
* @LastEditTime: 2024-02-29 03:17:36
*/
#ifndef SENSORMANAGER_H
......
src/camera_bev_infer/src/vision_transform/vision_3d.cpp
View file @ 6150ca42
......@@ -5,7 +5,7 @@
* @Date: 2023-09-03 03:13:41
* @email: xin.wang@waytous.com
* @LastEditors: wxin
* @LastEditTime: 2024-01-18 04:25:22
* @LastEditTime: 2024-02-28 10:32:38
*/
......@@ -714,7 +714,6 @@ cv::RotatedRect Visione_3D::getObjectAreaRect(const cv::Mat& imgBin, const cv::
}
}
}
if( vPoints.size() > 3)
{
rRect = minAreaRect(vPoints);
......@@ -963,10 +962,13 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
}
// std::cout<<" instance find table vision "<<double(clock()-start_)/CLOCKS_PER_SEC<<"s"<<std::endl; //输出时间（单位：ｓ）
float project_ratio = project_count / (mask_count + 1e-5);
bool valid = validTracks.find(obj.track_id) != validTracks.end();
// std::cout<<"obj: "<< obj.name <<" with project ratio: "<< project_ratio << ", " << valid << std::endl;
if(Tracks.find(obj.track_id) == Tracks.end()){
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;
if(valid){
validTracks[obj.track_id] = 0; // update
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);
......@@ -989,82 +991,57 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
insobj.fConfidence = obj.fConfidence;
vGrid.vInsObjs.push_back(insobj);
obj.valid = obj.valid + 1;
// std::cout<< obj.track_id<<"valid----------------"<<std::endl;
}else{
// std::cout<< obj.track_id<<"depth----------------"<<std::endl;
return -1; // depth estimination
}
}else{
if(pointEnough)
{
// 静态目标根据高度差过滤误检
bool filtered = false;
// if(std::find(perception::camera::StaticTypes.begin(), perception::camera::StaticTypes.end(), obj.name)
// != perception::camera::StaticTypes.end())
if(obj.name == "stone")
{
// 过滤多余点
auto bundingRect = insobj.rrc.boundingRect2f();
for(auto iter = points3d.begin(); iter != points3d.end();){
bool in_rect = ((iter->x <= bundingRect.x+bundingRect.width) && (iter->x >= bundingRect.x) &&
(iter->y <= bundingRect.y+bundingRect.height) && (iter->y >= bundingRect.y));
if(!in_rect){
iter = points3d.erase(iter);
}else{
iter++;
}
// 过滤多余点
auto bundingRect = insobj.rrc.boundingRect2f();
for(auto iter = points3d.begin(); iter != points3d.end();){
bool in_rect = ((iter->x <= bundingRect.x+bundingRect.width) && (iter->x >= bundingRect.x) &&
(iter->y <= bundingRect.y+bundingRect.height) && (iter->y >= bundingRect.y));
if(!in_rect){
iter = points3d.erase(iter);
}else{
iter++;
}
// 获得高低差
float min_z = 1e5, max_z = -1e5;
for(auto& p3d_: points3d){
if(p3d_.z > max_z){
max_z = p3d_.z;
}
if(p3d_.z < min_z){
min_z = p3d_.z;
}
}
// 获得高低差
float min_z = 1e5, max_z = -1e5;
for(auto& p3d_: points3d){
if(p3d_.z > max_z){
max_z = p3d_.z;
}
filtered = (max_z - min_z < obstacle_height_threshold * 0.01);
if(!filtered){
validTracks[obj.track_id] = 0; // init
std::cout << "------------------------ "<< obj.track_id <<" stone with "<<points3d.size() << " points, height diff: " << max_z - min_z << "m." << std::endl;
if(p3d_.z < min_z){
min_z = p3d_.z;
}
/*else{// 扬尘遮挡，点少
if(unvalidTracks.find(obj.track_id) != unvalidTracks.end()){
if(project_ratio < 0.05 && obj.fConfidence > 0.5){
unvalidTracks[obj.track_id].second++;
}
// 长时间跟踪上，但是未被激活，用深度估计激活
if(unvalidTracks[obj.track_id].second >= 10){
unvalidTracks.erase(obj.track_id);
validTracks[obj.track_id] = 0; // init
return -1; // using depth estimination
}
}else if(project_ratio < 0.05 && obj.fConfidence > 0.5){ // 当遮挡比例大时
unvalidTracks[obj.track_id] = std::make_pair(0, 0);
}
}*/
}
// 高差大于阈值输出
bool filtered = (max_z - min_z < obstacle_height_threshold * 0.01) && (max_z > obstacle_height_threshold * 0.01 * 0.5);
if(!filtered){
insobj.rc = obj.rc;
insobj.color = uColor;
insobj.names = obj.name;
insobj.fConfidence = obj.fConfidence;
vGrid.vInsObjs.push_back(insobj);
obj.valid = obj.valid + 1;
}
}else{
if(unvalidTracks.find(obj.track_id) != unvalidTracks.end()){
unvalidTracks[obj.track_id].second++;
// 长时间跟踪上，但是未被激活，用深度估计激活
// if(unvalidTracks[obj.track_id].second >= 10){
// unvalidTracks.erase(obj.track_id);
// validTracks[obj.track_id] = 0; // init
// return -1; // using depth estimination
// }
}else{
unvalidTracks[obj.track_id] = std::make_pair(0, 0);
Tracks[obj.track_id].valid_frame_num++; // add
std::cout << "------------------------ "<< obj.track_id <<": " << obj.name <<" with "<<points3d.size() <<
" points, height diff: " << max_z - min_z << "m, max-z: " << max_z << "m." << std::endl;
if(Tracks[obj.track_id].valid_frame_num >= 2){
Tracks[obj.track_id].valid = true;
insobj.rc = obj.rc;
insobj.color = uColor;
insobj.names = obj.name;
insobj.fConfidence = obj.fConfidence;
vGrid.vInsObjs.push_back(insobj);
obj.valid = obj.valid + 1;
}
}
// point enough, filtered by
}
// todo, point not enough
}
return 0;
......@@ -1139,29 +1116,17 @@ int Visione_3D::Process_objects(const cv::Mat& image, const cv::Mat& depth, Clou
Process_Segmantic_Objects(obj, point_index_in_camera, pcl_cloud, vGrid);
}
}
// update and delete valid stones
for(auto t = validTracks.begin(); t != validTracks.end();){
t->second++;
// std::cout<<t->first<<","<<t->second<<std::endl;
if(t->second > 30){
validTracks.erase(t++);
// update and delete tracks
for(auto t=Tracks.begin(); t != Tracks.end();){
t->second.last_update_age++;
t->second.tracked_age++;
if(t->second.last_update_age > 30){
Tracks.erase(t++);
}else{
t++;
}
}
// std::cout<<"-----valid stones:" << validTracks.size() << std::endl;
// update and delete unvalid stones
for(auto t = unvalidTracks.begin(); t != unvalidTracks.end();){
t->second.first++;
// std::cout<<t->first<<","<<t->second.first<<","<<t->second.second<<std::endl;
if(t->second.first > 30){
unvalidTracks.erase(t++);
}else{
t++;
}
}
// std::cout<<"-----unvalid stones:" << unvalidTracks.size() << std::endl;
// 将静态目标投影到栅格地图中
pcl::PointXYZI bev_point;
cv::Point2f pt2f;
......
src/camera_bev_infer/src/vision_transform/vision_3d.h
View file @ 6150ca42
......@@ -5,7 +5,7 @@
* @Date: 2023-04-13 14:37:39
* @emal: qingshui.cheng@waytous.com
* @LastEditors: wxin
* @LastEditTime: 2023-12-04 06:11:33
* @LastEditTime: 2024-02-28 09:14:51
*/
#ifndef VISION_3D_H
......@@ -214,10 +214,17 @@ private:
std::string current_frame_id = "Lidar";
bool vis_project = false;
struct STrack{
bool valid = false;
int valid_frame_num = 0;
int tracked_age = 0;
int last_update_age = 0;
};
// 保存跟踪后高度够的落石, id: last_update_age
std::map<int, int> validTracks;
// 保存跟踪的，但是高度不够，未被激活的落石, id: (tracked_age, last_update_age)
std::map<int, std::pair<int, int>> unvalidTracks;
std::map<int, STrack> Tracks;
// 保存跟踪的，但是高度不够，未被激活的落石, id: (tracked_age, last_update_age)
// std::map<int, std::pair<int, int>> unvalidTracks;
cv::Mat m_matrixQ;
Eigen::Matrix4d m_lidar2camera;
// Eigen::Matrix4d m_lidar2odom;
......
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