Skip to content

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

add calib and transform bbox3d

parent 9c3f8668
Hide whitespace changes
Inline Side-by-side
Showing with 45 additions and 28 deletions
src/camera_bev_infer/configs/CameraBevParam.yaml
View file @ 7842dfc3
......@@ -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
View file @ 7842dfc3
......@@ -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
View file @ 7842dfc3
......@@ -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
View file @ 7842dfc3
......@@ -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:
......
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