粗大メモ置き場

個人用,たまーに来訪者を意識する雑記メモ

Read yaml file with yaml-cpp in ROS C++ / yamlファイルをC++で開く

This article both contains both Japanese and English explanations.
需要とか考えた結果、英語と日本語で書きます。

Goal: Read .yaml calibration files and then publish camera info

以下のようなyamlファイルをC++で読んでPublishするのが目的です。

image_width: 640
image_height: 480
camera_name: narrow_stereo/left
camera_matrix:
  rows: 3
  cols: 3
  data: [438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.361976, 0.110510, 0.001014, 0.000505, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
projection_matrix:
  rows: 3
  cols: 4
  data: [393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]


And My environment is ROS kinetic and Ubuntu16.04 on amd64 cpu.
環境は上に示したとおりです。

Method: Using yaml-cpp

Installation

Download and make from source. Follow their installation guide.
GitHub - jbeder/yaml-cpp: A YAML parser and emitter in C++

ROS setup (Kinetic)

Here shows how to add dependency on rosmake.
yaml_cpp - ROS Wiki

But I use catkin_make, so I have to change the "package.xml" and "CmakeList" instead.

In your CmakeList in project, you should add

target_link_libraries(${PROJECT_NAME} yaml-cpp)

If you have already added linker, you can write as below:

target_link_libraries(${PROJECT_NAME} <your_already_added_link> yaml-cpp)

Then open your package.xml and add dependency as below:

...
  <build_depend>common_rosdeps</build_depend>
...
  <run_depend>common_rosdeps</run_depend>
...


Now you can edit your project.

Usage

I recommend you to understand what json/yaml format represents and how you read the data in python for introduction.
まずはPythonなどでファイルを開く方法を調べたほうが理解がスムーズに行くと思います。

Sample code

You need to include

#include <yaml-cpp/yaml.h>

and then you can read files like below:

// Load file
YAML::Node lconf = YAML::LoadFile(filename.yaml);

// how to read data
int iw = lconf["image_width"].as<int>();
std::string model = lconf["distortion_model"].as<std::string>();
std::vector<double> distdata = lconf["distortion_coefficients"]["data"].as<std::vector<double>>();

If the type of value is different from you declared, you will see the error about that.

Application: read yaml file and publish camera_info

Here show the sequence I publish these parameter in ros.

    // fill the camera data before running
    sensor_msgs::CameraInfoPtr lcam(new sensor_msgs::CameraInfo());
    sensor_msgs::CameraInfoPtr rcam(new sensor_msgs::CameraInfo());

    // read calib file
    std::string fnamel,fnamer;
    
    if( argc != 3 ){ // argc should be 2 for correct execution
        fnamel = "/home/ubuntu/calibres/left1.yml";
        fnamer = "/home/ubuntu/calibres/right1.yml";
        std::cout << "Load defaults! " <<  std::endl;
    }else {
        fnamel=argv[1];
        fnamer=argv[2];
        std::cout << "Now load..." << " " << fnamel << " and " << fnamer << std::endl;
    }
    
    YAML::Node lconf = YAML::LoadFile(fnamel);
    YAML::Node rconf = YAML::LoadFile(fnamer);
    
    // start read left camera configure! 
    lcam->width = lconf["image_width"].as<int>();
    lcam->height = lconf["image_height"].as<int>();
    lcam->distortion_model = lconf["distortion_model"].as<std::string>();    
    // get value
    std::vector<double> dd,kk,rr,pp,buf;
    dd = lconf["distortion_coefficients"]["data"].as<std::vector<double>>();
    kk = lconf["camera_matrix"]["data"].as<std::vector<double>>();
    rr = lconf["rectification_matrix"]["data"].as<std::vector<double>>();
    pp = lconf["projection_matrix"]["data"].as<std::vector<double>>();
    // conversion
    boost::array<double, 9ul> kl;
    std::memcpy(&kl[0], &kk[0], sizeof(double)*9);
    boost::array<double, 9ul> rl;
    std::memcpy(&rl[0], &rr[0], sizeof(double)*9);
    boost::array<double, 12ul> pl;
    std::memcpy(&pl[0], &pp[0], sizeof(double)*12);
    // put value
    lcam->D =  dd; 
    lcam->K =  kl; 
    lcam->R =  rl; 
    lcam->P =  pl;

Method: Using OpenCV builtin function

There is a good hint for another solution
sourishghosh.com