【学习记录】提取Kalibr生成的YAML文件内外参

Kalibr标定的结果生成的是xxx.yaml文件。之前一直是手工拷贝参数到opencv中进行存储，最近实在是有需求，简单学习了下yaml文件的读取。仅用作个人学习总结。

0. 参考资料

yaml官方资料：https://yaml.org/spec/1.2.2
yaml-cpp入门csdn：https://blog.csdn.net/briblue/article/details/89515470
yaml-cpp的官方教程：[https://github.com/jbeder/yaml-cpp/wiki/Tutorial](https://github.com/jbeder/yaml-cpp/wiki/Tutorial0

1. 基本概念

yaml数据格式包括：scalar, sequence, map等格式，不同格式的显示方式不同。
对照kalibr标定后的文件，如下：

cam1:
  T_cn_cnm1:
  - [0.9999338057564335, -0.0055826230507691, 0.010060736818260632, 0.0835663824710185]
  - [0.005342923274402102, 0.9997049166163952, 0.02369668466027778, 0.0027964232339661396]
  - [-0.010190057720010024, -0.023641362331257763, 0.9996685694322811, -0.01053488339904625]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.3754631018222977, 0.12358593641003396, 0.00273931728537412,
    0.0031343839102278646]
  distortion_model: radtan
  intrinsics: [266.73407158843924, 266.0456492113577, 171.75122828472826, 109.55506992335957]
  resolution: [346, 260]
  rostopic: /cam1/image_raw

用yaml-cpp读取这些数据，然后查看分别是什么类型，发现：

• cam1是Map类型
• camera_model/rostopic是scalar类型
• cam_overlaps/distortion_coeffs/intrinsics/resolution是sequence类型
• T_cn_cnm1也是sequence类型，只不过比较特殊，属于Flow的表示形式，但依旧可以用iterator方式读取

    cout << "kalibr type: " << kalibr.Type() << endl;               // 4, map
    cout << "cam0 type: " << kalibr["cam0"].Type() << endl;         // 4, map
    cout << "distortion_coeffs, type: " << kalibr["cam0"]["distortion_coeffs"].Type() << endl;  // 3, seq
    cout << "intrinsics, type: " << kalibr["cam0"]["intrinsics"].Type() << endl;                // 3, seq
    cout << "resolution, type: " << kalibr["cam0"]["resolution"].Type() << endl;                // 3, seq
    cout << "T_cn_cnm1, type: " << kalibr["cam1"]["T_cn_cnm1"].Type() << endl;                  // 3, seq
    cout << "Camera model, type: " << kalibr["cam1"]["camera_model"].Type() << endl;            // 2, scalar    

2. 读取与写入

我希望将Flow表示的Sequence转成更方便的四元数+vector方式，以最简单的Sequence方式存储。因此借用了Eigen库，最终代码如下：

#include <iostream>
#include <fstream>
#include "yaml-cpp/yaml.h"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

// 将YAML的node转成Eigen的Matrix4d
inline void node2Matrix4d(const YAML::Node& n, Eigen::Matrix4d& T){
    T = Eigen::Matrix4d::Identity();
    T(0,0) = n[0][0].as<double>();
    T(0,1) = n[0][1].as<double>();
	// 省略部分代码...
}
// 将Matrix4d转成vector7: qx,qy,qz,qw,tx,ty,tz
inline void format(const Eigen::Matrix4d& T, vector<double>& vec){
    Eigen::Matrix3d R = T.topLeftCorner(3, 3);
    Eigen::Vector3d t = T.topLeftCorner(3, 1);
    Eigen::Quaterniond q(R);
    vec.resize(7);
    vec[0] = q.x(); vec[1] = q.y(); vec[2] = q.z(); vec[3] = q.w();
    vec[4] = t(0); vec[5] = t(1); vec[6] = t(2);
}
// 省略部分代码...
YAML::Node kalibr = YAML::LoadFile(input_file);
Eigen::Matrix4d T_0_1;
node2Matrix4d(kalibr["cam1"]["T_cn_cnm1"], T_0_1);
vector<double> vec01;
format(T_0_1, vec01);

YAML::Emitter emit;					// 采用Emitter方式输出
emit << YAML::BeginMap << YAML::Key << "cam1";
emit << YAML::BeginMap << YAML::Key << "resolution" << YAML::Value << kalibr["cam1"]["resolution"] << YAML::EndMap;
emit << YAML::BeginMap << YAML::Key << "intrinsics" << YAML::Value << kalibr["cam1"]["intrinsics"] << YAML::EndMap;
emit << YAML::BeginMap << YAML::Key << "distortion_coeffs" << YAML::Value << kalibr["cam1"]["distortion_coeffs"] << YAML::EndMap;
emit << YAML::BeginMap << YAML::Key << "T01"<< YAML::Value << YAML::Flow << vec01 << YAML::EndMap;
emit << YAML::EndMap;

只不过最终输出结果带有问号?和---等，自己没有搞清楚到底怎么样输出合适的结果，YAML太多内容了。但手动删掉也能凑合解决问题。

最终（手动删掉一些符号）的显示结果如下，这样至少应该可以方便读取了。

就这样吧。记录大半天的时光。

采用OpenCV的FileStorage存储

后来发现采用opencv的filestorage写入，更方便。
主要参考：【C++】42.使用YAML文件进行参数配置、读取与生成YAML文件
FileStorage

inline vector<double> cvtDouble4(const YAML::Node& n){
    vector<double> out;
    for (int i = 0; i < 4; ++i)
        out.push_back(n[i].as<double>());
    return out;
}
template<typename T>
inline void printVec(const vector<T> vec){
    for(auto v:vec)
        cout << v << ", ";
    cout << endl;
}
--------------
// Test OpenCV writer.
vector<vector<int>> v_reso;
vector<vector<double>> v_K, v_D;
for(int i=0; i<CameraNumber; ++i){
    string camid = "cam" + to_string(i);
    v_reso.push_back(cvtInt2(kalibr[camid]["resolution"]));
    v_K.push_back(cvtDouble4(kalibr[camid]["intrinsics"]));
    v_D.push_back(cvtDouble4(kalibr[camid]["distortion_coeffs"]));
}

cv::FileStorage fo(output_file, cv::FileStorage::WRITE);
const vector<string> camera_names{"rgb", "davis", "celex", "prophesee"};
for(int i=0; i<CameraNumber; ++i){
    fo << "cam" + to_string(i)
       << "{"
       << "name" << camera_names[i]
       << "resolution"
       << "[:" << v_reso[i][0] << v_reso[i][1] << "]"
       << "intrinsics"
       << "[:" << v_K[i][0] << v_K[i][1] << v_K[i][2] << v_K[i][3] << "]"
       << "distortion"
       << "[:" << v_D[i][0] << v_D[i][1] << v_D[i][2] << v_D[i][3] << "]";
    if (i != 0){
        fo << "T0" + to_string(i) << "TODO";
    }
    fo << "}";
}
fo.release();

FileStorage读取文件

template<typename T>
inline void cvtNode2Vec(cv::FileNode n, vector<T>& vec){
    vec.resize(0);
    for(auto iter = n.begin(); iter!=n.end(); ++iter)
    vec.push_back(*iter);
}

int main(int argc, char **argv){
    cv::FileStorage fs(input_file, cv::FileStorage::READ);
    vector<vector<int>> v_reso;
    vector<vector<double>> v_D, v_K;
    for(int i=0; i<CameraNumber; ++i){
        vector<int> r;
        vector<double> d, k;
        string camid = "cam" + to_string(i);
        cvtNode2Vec(fs[camid]["resolution"], r);
        cvtNode2Vec(fs[camid]["distortion"], d);
        cvtNode2Vec(fs[camid]["intrinsics"], k);
        v_reso.push_back(r);
        v_D.push_back(d);
        v_K.push_back(k);
    }