Merge pull request #17 from utkarshjp7/master

Publish camera info topic with complete information.

Author: Vikrant Shah

README.md

@@ -89,13 +89,19 @@ This is the names that would be given to the cameras for filenames and rostopics
   Secs to wait in the deinit/init sequence
 
 ### Camera info message details
+* ~image_width (int)
+* ~image_height (int)
 * ~distortion_model (string)  
   Distortion model for the camera calibration.  Typical is 'plumb_bob'
 * ~distortion_coeffs (array of arrays)  
   Distortion coefficients of all the cameras in the array.  Must match the number of cam_ids provided.
 * ~intrinsic_coeff (array of arrays)  
   Intrinsic coefficients of all the cameras in the array.  Must match the number of cam_ids provided.
-Specified as [fx  0 cx 0 fy cy 0  0  1]
+  Specified as [fx  0 cx 0 fy cy 0  0  1]
+* ~projection_coeffs (array of arrays)  
+  Projection coefficients of all the cameras in the array.  Must match the number of cam_ids provided.
+* ~rectification_coeffs (array of arrays)  
+  Rectification coefficients of all the cameras in the array.  Must match the number of cam_ids provided.
 
 ## Multicamera Master-Slave Setup
 When using multiple cameras, we have found that the only way to keep images between different cameras synched is by using a master-slave setup using the GPIO connector. So this is the only way we support multicamera operation with this code. A general guide for multi camera setup is available at https://www.ptgrey.com/tan/11052, however note that we use a slightly different setup with our package.

include/spinnaker_sdk_camera_driver/capture.h

@@ -75,6 +75,8 @@ namespace acquisition {
         vector< vector<Mat> > mem_frames_;
         vector<vector<double>> intrinsic_coeff_vec_;
         vector<vector<double>> distortion_coeff_vec_;
+        vector<vector<double>> rect_coeff_vec_;
+        vector<vector<double>> proj_coeff_vec_;
         vector<string> imageNames;
 
         string path_;

params/multi-cam_example.yaml

@@ -21,6 +21,8 @@ skip: 5
 
 #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.06001035588294054, 0.3466585291091072, -1.0932356940133992, 3.4511959617065253]
@@ -35,3 +37,18 @@ intrinsic_coeffs:
 - [1872.6246599679714, 0.0, 594.706935782809, 0.0, 1873.4112078551445, 497.511707550892, 0.0, 0.0, 1.0]
 - [1867.922201234431, 0.0, 565.5570110679519, 0.0, 1868.1730959576214, 485.76092237501405, 0.0, 0.0, 1.0]
 - [1880.7646202582603, 0.0, 582.3098456998288, 0.0, 1883.4340813437482, 483.8937525066349, 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]
+- [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]
+- [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]
+- [703.910323, 0.000000, 815.113302, 0.000000, 0.000000, 958.319231, 636.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
+- [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 911.742312, 811.121993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
+- [923.700443, 0.000000, 973.847232, 0.000000, 0.000000, 1100.763631, 832.341983, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
+- [800.784635, 0.000000, 983.78252, 0.000000, 0.000000, 903.736234, 798.561973, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
+

src/capture.cpp

@@ -162,19 +162,32 @@ void acquisition::Capture::load_cameras() {
 
                 if (PUBLISH_CAM_INFO_){
                     sensor_msgs::CameraInfo ci_msg;
-                    
-                    ci_msg.height = 1024;
-                    ci_msg.width = 1280;
-                    ci_msg.distortion_model = "plumb_bob";
-                    ci_msg.D = distortion_coeff_vec_[j];
+                    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";
+                    ci_msg.width = image_width;
+                    ci_msg.height = image_height;
+                    ci_msg.distortion_model = distortion_model;
                     ci_msg.binning_x = binning_;
                     ci_msg.binning_y = binning_;
+                    
+                    ci_msg.D = distortion_coeff_vec_[j];
                     for (int count = 0; count<intrinsic_coeff_vec_[j].size();count++)
                         ci_msg.K[count] = intrinsic_coeff_vec_[j][count];
-                    ci_msg.header.frame_id = "cam_"+to_string(j)+"_optical_frame";
-                    ci_msg.P = {intrinsic_coeff_vec_[j][0], intrinsic_coeff_vec_[j][1],intrinsic_coeff_vec_[j][2], 0,
-                                intrinsic_coeff_vec_[j][3],intrinsic_coeff_vec_[j][4],intrinsic_coeff_vec_[j][5],0,
-                                intrinsic_coeff_vec_[j][6],intrinsic_coeff_vec_[j][7],intrinsic_coeff_vec_[j][8],0};
+                    
+                    if (!rect_coeff_vec_.empty())
+                        ci_msg.R = {rect_coeff_vec_[j][0], rect_coeff_vec_[j][1], rect_coeff_vec_[j][2],
+                                    rect_coeff_vec_[j][3], rect_coeff_vec_[j][4], rect_coeff_vec_[j][5],
+                                    rect_coeff_vec_[j][6], rect_coeff_vec_[j][7], rect_coeff_vec_[j][8]};
+                    if (!proj_coeff_vec_.empty())
+                        ci_msg.P = {proj_coeff_vec_[j][0], proj_coeff_vec_[j][1], proj_coeff_vec_[j][2], proj_coeff_vec_[j][3],
+                                    proj_coeff_vec_[j][4], proj_coeff_vec_[j][5], proj_coeff_vec_[j][6], proj_coeff_vec_[j][7],
+                                    proj_coeff_vec_[j][8], proj_coeff_vec_[j][9], proj_coeff_vec_[j][10], proj_coeff_vec_[j][11]};
 
                     cam_info_msgs.push_back(ci_msg);
                 }
@@ -386,6 +399,42 @@ void acquisition::Capture::read_parameters() {
             distort_list_provided = true;
         }
     }
+    
+    XmlRpc::XmlRpcValue rect_list;
+
+    if (nh_pvt_.getParam("rectification_coeffs", rect_list)) {
+        ROS_INFO("  Camera Rectification Paramters:");
+        ROS_ASSERT_MSG(rect_list.size() == num_ids,"If rectification_coeffs are provided, they should be the same number as cam_ids and should correspond in order!");
+        for (int i=0; i<rect_list.size(); i++){
+            std::vector<double> rect;
+            String rect_str="";
+            for (int j=0; j<rect_list[i].size(); j++){
+                ROS_ASSERT_MSG(rect_list[i][j].getType()== XmlRpc::XmlRpcValue::TypeDouble,"Make sure all numbers are entered as doubles eg. 0.0 or 1.1");
+                rect.push_back(static_cast<double>(rect_list[i][j]));
+                rect_str = rect_str +to_string(rect[j])+" ";
+            }
+            rect_coeff_vec_.push_back(rect);
+            ROS_INFO_STREAM("   "<< rect_str );
+        }
+    }
+    
+    XmlRpc::XmlRpcValue proj_list;
+
+    if (nh_pvt_.getParam("projection_coeffs", proj_list)) {
+        ROS_INFO("  Camera Projection Paramters:");
+        ROS_ASSERT_MSG(proj_list.size() == num_ids,"If projection_coeffs are provided, they should be the same number as cam_ids and should correspond in order!");
+        for (int i=0; i<proj_list.size(); i++){
+            std::vector<double> proj;
+            String proj_str="";
+            for (int j=0; j<proj_list[i].size(); j++){
+                ROS_ASSERT_MSG(proj_list[i][j].getType()== XmlRpc::XmlRpcValue::TypeDouble,"Make sure all numbers are entered as doubles eg. 0.0 or 1.1");
+                proj.push_back(static_cast<double>(proj_list[i][j]));
+                proj_str = proj_str +to_string(proj[j])+" ";
+            }
+            proj_coeff_vec_.push_back(proj);
+            ROS_INFO_STREAM("   "<< proj_str );
+        }
+    }
 
     PUBLISH_CAM_INFO_ = intrinsics_list_provided && distort_list_provided;
     if (PUBLISH_CAM_INFO_)