Reading Stereo Camera Calibration using rosparam

edit

Hi all!

I have a YAML file which contains calibration matrices for a stereo pair of cameras. I just can't figure out how to get the calibration data into a CameraInfoManager instance. The YAML file looks like this:

# Transform that takes a point in lidar frame and puts it in the left DAVIS camera frame.
T_cam0_lidar:
  - [ 0.01140786, -0.99985524,  0.01262402,  0.03651404]
  - [ 0.04312291, -0.01212116, -0.99899624, -0.08637514]
  - [ 0.99900464,  0.0119408 ,  0.04297839, -0.12882002]
  - [ 0.        ,  0.        ,  0.        ,  1.        ]
cam0:
  # Transform that takes a point in the left DAVIS IMU frame to the left DAVIS camera frame.
  T_cam_imu:
  - [ 0.9998771896957381, -0.015128404695721132, -0.004091075349566317, -0.002760732198752687]
  - [ 0.015081690576531692,  0.9998233340761795, -0.011217987615716542, -0.001652095229536009]
  - [ 0.004260062852482415,  0.011154909598696812,  0.99992871, -0.017829494499329843]
  - [0.0, 0.0, 0.0, 1.0]
  projection_matrix:
  - [199.6530123165822, 0.0, 177.43276376280926, 0.0]
  - [0.0, 199.6530123165822, 126.81215684365904, 0.0]
  - [0.0, 0.0, 1.0, 0.0]
  rectification_matrix:
  - [0.999877311526236, 0.015019439766575743, -0.004447282784398257]
  - [-0.014996983873604017, 0.9998748347535599, 0.005040367172759556]
  - [0.004522429630305261, -0.004973052949604937, 0.9999774079320989]
  camera_model: pinhole
  distortion_coeffs: [-0.048031442223833355, 0.011330957517194437, -0.055378166304281135,
    0.021500973881459395]
  distortion_model: equidistant
  intrinsics: [226.38018519795807, 226.15002947047415, 173.6470807871759, 133.73271487507847]
  resolution: [346, 260]
  rostopic: /davis/left/image_raw
cam1:
  # Transform that takes a point in the left DAVIS IMU frame to the right DAVIS camera frame.
  T_cam_imu:
  - [0.9999459669775138, -0.002658051013083418, -0.010049770654903653, -0.10052351879548456]
  - [0.002628365498611729, 0.9999921475216119, -0.0029659045702786734, -0.002149609013368439]
  - [0.01005757526494452, 0.0029393298430320275, 0.9999451012529955, -0.01780039434280798]
  - [0.0, 0.0, 0.0, 1.0]
  # Transform that takes a point in the left DAVIS camera frame to the right DAVIS camera frame.
  T_cn_cnm1:
  - [0.9999285439274112, 0.011088072985503046, -0.004467849222081981, -0.09988137641750752]
  - [-0.011042817783611191, 0.9998887260774646, 0.01002953830336461, -0.0003927067773089277]
  - [0.004578560319692358, -0.009979483987103495, 0.9999397215256256, 1.8880107752680777e-06]
  - [0.0, 0.0, 0.0, 1.0]
  projection_matrix:
  - [199.6530123165822, 0.0, 177.43276376280926, -19.941771812941038]
  - [0.0, 199.6530123165822, 126.81215684365904, 0.0]
  - [0.0, 0.0, 1.0, 0.0]
  rectification_matrix:
  - [0.9999922706537476, 0.003931701344419404, -1.890238450965101e-05]
  - [-0.003931746704476347, 0.9999797362744968, -0.005006836150689904]
  - [-7.83382948021244e-07, 0.0050068717705076754, 0.9999874655386736]
  camera_model: pinhole
  distortion_coeffs: [-0.04846669832871334, 0.010092844338123635, -0.04293073765014637,
    0.005194706897326005]
  distortion_model: equidistant
  intrinsics: [226.0181418548734, 225.7869434267677, 174.5433576736815, 124.21627572590607]
  resolution: [346, 260]
  rostopic: /davis/right/image_raw

and I am able to access the data through a nodehandle by adding a rosparam to my launch file:

<rosparam file="/home/timo/.ros/camera_info/camchain-imucam-indoor_flying.yaml" command="load" ns="stereo_params"/>

and

std::vector<double> test;
nh.getParam("event_stereo/stereo_params/cam0/projection_matrix", test);

This works fine, so I am able to access what is in the YAML through the nodehandle. What I'd like to do now, is actually load the matrices into a image_geometry::PinholeCameraModel so I can rectify incoming images and to load the camera extrinsics in order to do some stereo camera operations. I've tried all sorts of things, increasingly inelegant, but there must be an easy way... Many thanks in advance!