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
- Method: Using yaml-cpp
- Usage
- Application: read yaml file and publish camera_info
- Method: Using OpenCV builtin function
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