From a903b0b6f941460f1e87b0a2d523796deb96509c Mon Sep 17 00:00:00 2001 From: mithun Varma Diddi Date: Wed, 19 Jun 2019 17:40:26 -0400 Subject: [PATCH] fixes issue when more cameras are plugged into host than count of specified cam_ids, node will not die, and now node publishes empty intrinsics in camerainfo msg when not provided, a bug from pr #33 where default was to not publish info msg (#36) fixes issue when more cameras are plugged into host than count of specified cam_ids, node will not die, and now node publishes empty intrinsics in camerainfo msg when not provided, a bug from pr #33 where default was to not publish info msg (#36) --- params/stereo_camera_example.yaml | 42 ++++++++++++++++++++++++ params/test_params.yaml | 4 +-- src/capture.cpp | 53 +++++++++++++++++++------------ 3 files changed, 76 insertions(+), 23 deletions(-) create mode 100644 params/stereo_camera_example.yaml diff --git a/params/stereo_camera_example.yaml b/params/stereo_camera_example.yaml new file mode 100644 index 0000000..49cc1d6 --- /dev/null +++ b/params/stereo_camera_example.yaml @@ -0,0 +1,42 @@ +cam_ids: +- 17197559 +- 17197547 +cam_aliases: +- cam0 +- cam1 +master_cam: 17197559 +skip: 20 +delay: 1.0 + +# Assign all the follwing via launch file to prevent confusion and conflict + +#save_path: ~/projects/data +#save_type: .bmp #binary or .tiff or .bmp +#binning: 1 # going from 2 to 1 requires cameras to be unplugged and replugged +#color: false +#frames: 50 +#soft_framerate: 20 # this frame rate reflects to the software frame rate set using ros::rate +#exp: 997 +#to_ros: true #When to_ros is not selected, but live is selected, pressing 'space' exports single image to ROS + +#Camera info message details +distortion_model: plumb_bob +image_height: 1536 +image_width: 2048 +distortion_coeffs: +- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] +- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] + + +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: +- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0] +- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0] + +rectification_coeffs: +- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] + +projection_coeffs: +- [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] +- [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/params/test_params.yaml b/params/test_params.yaml index 707e52d..55d7fc3 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,8 +1,8 @@ cam_ids: -- 18080155 +- 17197559 cam_aliases: - cam0 -master_cam: 18080155 +master_cam: 17197559 skip: 20 delay: 1.0 diff --git a/src/capture.cpp b/src/capture.cpp index 3bde1bf..863d958 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -251,24 +251,30 @@ void acquisition::Capture::load_cameras() { //camera_info_pubs.push_back(nh_.advertise("camera_array/"+cam_names_[j]+"/camera_info", 1)); img_msgs.push_back(sensor_msgs::ImagePtr()); + + sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo()); + int image_width = 0; + int image_height = 0; + std::string distortion_model = ""; + nh_pvt_.getParam("image_height", image_height); + nh_pvt_.getParam("image_width", image_width); + nh_pvt_.getParam("distortion_model", distortion_model); + ci_msg->header.frame_id = "cam_"+to_string(j)+"_optical_frame"; + // full resolution image_size + ci_msg->height = image_height; + ci_msg->width = image_width; + // distortion + ci_msg->distortion_model = distortion_model; + // binning + ci_msg->binning_x = binning_; + ci_msg->binning_y = binning_; + if (PUBLISH_CAM_INFO_){ - sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo()); - int image_width = 0; - int image_height = 0; - std::string distortion_model = ""; - nh_pvt_.getParam("image_height", image_height); - nh_pvt_.getParam("image_width", image_width); - nh_pvt_.getParam("distortion_model", distortion_model); - ci_msg->header.frame_id = "cam_"+to_string(j)+"_optical_frame"; - // full resolution image_size - ci_msg->height = image_height; - ci_msg->width = image_width; - // distortion - ci_msg->distortion_model = distortion_model; ci_msg->D = distortion_coeff_vec_[j]; // intrinsic coefficients - for (int count = 0; countK[count] = intrinsic_coeff_vec_[j][count]; + } // Rectification matrix if (!rect_coeff_vec_.empty()) ci_msg->R = { @@ -287,7 +293,8 @@ void acquisition::Capture::load_cameras() { proj_coeff_vec_[j][8], proj_coeff_vec_[j][9], proj_coeff_vec_[j][10], proj_coeff_vec_[j][11]}; } - else if(numCameras_ == 1){ + //else if(numCameras_ == 1){ + else if(cam_ids_.size() == 1){ // for case of monocular camera, P[1:3,1:3]=K ci_msg->P = { intrinsic_coeff_vec_[j][0], intrinsic_coeff_vec_[j][1], @@ -297,19 +304,23 @@ void acquisition::Capture::load_cameras() { intrinsic_coeff_vec_[j][6], intrinsic_coeff_vec_[j][7], intrinsic_coeff_vec_[j][8], 0}; } - // binning - ci_msg->binning_x = binning_; - ci_msg->binning_y = binning_; - - cam_info_msgs.push_back(ci_msg); } + + cam_info_msgs.push_back(ci_msg); + cam_counter++; + } } if (!current_cam_found) ROS_WARN_STREAM(" Camera "<