Go to the documentation of this file.00001 #include "camera_calibration_parsers/parse_yml.h"
00002 #include <sensor_msgs/distortion_models.h>
00003 #include <yaml-cpp/yaml.h>
00004 #include <fstream>
00005 #include <ctime>
00006 #include <cassert>
00007 #include <cstring>
00008 #include <ros/console.h>
00009
00010 namespace camera_calibration_parsers {
00011
00013
00014 static const char CAM_YML_NAME[] = "camera_name";
00015 static const char WIDTH_YML_NAME[] = "image_width";
00016 static const char HEIGHT_YML_NAME[] = "image_height";
00017 static const char K_YML_NAME[] = "camera_matrix";
00018 static const char D_YML_NAME[] = "distortion_coefficients";
00019 static const char R_YML_NAME[] = "rectification_matrix";
00020 static const char P_YML_NAME[] = "projection_matrix";
00021 static const char DMODEL_YML_NAME[] = "distortion_model";
00022
00023 struct SimpleMatrix
00024 {
00025 int rows;
00026 int cols;
00027 double* data;
00028
00029 SimpleMatrix(int rows, int cols, double* data)
00030 : rows(rows), cols(cols), data(data)
00031 {}
00032 };
00033
00034 YAML::Emitter& operator << (YAML::Emitter& out, const SimpleMatrix& m)
00035 {
00036 out << YAML::BeginMap;
00037 out << YAML::Key << "rows" << YAML::Value << m.rows;
00038 out << YAML::Key << "cols" << YAML::Value << m.cols;
00039
00040 out << YAML::Key << "data" << YAML::Value;
00041 out << YAML::Flow;
00042 out << YAML::BeginSeq;
00043 for (int i = 0; i < m.rows*m.cols; ++i)
00044 out << m.data[i];
00045 out << YAML::EndSeq;
00046 out << YAML::EndMap;
00047 return out;
00048 }
00049
00050 void operator >> (const YAML::Node& node, SimpleMatrix& m)
00051 {
00052 int rows, cols;
00053 node["rows"] >> rows;
00054 assert(rows == m.rows);
00055 node["cols"] >> cols;
00056 assert(cols == m.cols);
00057 const YAML::Node& data = node["data"];
00058 for (int i = 0; i < rows*cols; ++i)
00059 data[i] >> m.data[i];
00060 }
00061
00063
00064 bool writeCalibrationYml(std::ostream& out, const std::string& camera_name,
00065 const sensor_msgs::CameraInfo& cam_info)
00066 {
00067 YAML::Emitter emitter;
00068 emitter << YAML::BeginMap;
00069
00070 #if 0
00071
00073 time_t raw_time;
00074 time( &raw_time );
00075 emitter << YAML::Key << "calibration_time";
00076 emitter << YAML::Value << asctime(localtime(&raw_time));
00077 #endif
00078
00079
00080 emitter << YAML::Key << WIDTH_YML_NAME << YAML::Value << (int)cam_info.width;
00081 emitter << YAML::Key << HEIGHT_YML_NAME << YAML::Value << (int)cam_info.height;
00082
00083
00084 emitter << YAML::Key << CAM_YML_NAME << YAML::Value << camera_name;
00085 emitter << YAML::Key << K_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.K[0]));
00086 emitter << YAML::Key << DMODEL_YML_NAME << YAML::Value << cam_info.distortion_model;
00087 emitter << YAML::Key << D_YML_NAME << YAML::Value << SimpleMatrix(1, cam_info.D.size(),
00088 const_cast<double*>(&cam_info.D[0]));
00089 emitter << YAML::Key << R_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.R[0]));
00090 emitter << YAML::Key << P_YML_NAME << YAML::Value << SimpleMatrix(3, 4, const_cast<double*>(&cam_info.P[0]));
00091
00092 emitter << YAML::EndMap;
00093
00094 out << emitter.c_str();
00095 return true;
00096 }
00097
00098 bool writeCalibrationYml(const std::string& file_name, const std::string& camera_name,
00099 const sensor_msgs::CameraInfo& cam_info)
00100 {
00101 std::ofstream out(file_name.c_str());
00102 if (!out.is_open())
00103 {
00104 ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str());
00105 return false;
00106 }
00107 return writeCalibrationYml(out, camera_name, cam_info);
00108 }
00109
00110 bool readCalibrationYml(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
00111 {
00112 try {
00113 YAML::Parser parser(in);
00114 if (!parser) {
00115 ROS_ERROR("Unable to create YAML parser for camera calibration");
00116 return false;
00117 }
00118 YAML::Node doc;
00119 parser.GetNextDocument(doc);
00120
00121 if (const YAML::Node* name_node = doc.FindValue(CAM_YML_NAME))
00122 *name_node >> camera_name;
00123 else
00124 camera_name = "unknown";
00125
00126 doc[WIDTH_YML_NAME] >> cam_info.width;
00127 doc[HEIGHT_YML_NAME] >> cam_info.height;
00128
00129
00130 SimpleMatrix K_(3, 3, &cam_info.K[0]);
00131 doc[K_YML_NAME] >> K_;
00132 SimpleMatrix R_(3, 3, &cam_info.R[0]);
00133 doc[R_YML_NAME] >> R_;
00134 SimpleMatrix P_(3, 4, &cam_info.P[0]);
00135 doc[P_YML_NAME] >> P_;
00136
00137
00138 if (const YAML::Node* model_node = doc.FindValue(DMODEL_YML_NAME)) {
00139 *model_node >> cam_info.distortion_model;
00140 }
00141 else {
00142
00143 cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00144 ROS_WARN("Camera calibration file did not specify distortion model, assuming plumb bob");
00145 }
00146 const YAML::Node& D_node = doc[D_YML_NAME];
00147 int D_rows, D_cols;
00148 D_node["rows"] >> D_rows;
00149 D_node["cols"] >> D_cols;
00150 const YAML::Node& D_data = D_node["data"];
00151 cam_info.D.resize(D_rows*D_cols);
00152 for (int i = 0; i < D_rows*D_cols; ++i)
00153 D_data[i] >> cam_info.D[i];
00154
00155 return true;
00156 }
00157 catch (YAML::Exception& e) {
00158 ROS_ERROR("Exception parsing YAML camera calibration:\n%s", e.what());
00159 return false;
00160 }
00161 }
00162
00163 bool readCalibrationYml(const std::string& file_name, std::string& camera_name,
00164 sensor_msgs::CameraInfo& cam_info)
00165 {
00166 std::ifstream fin(file_name.c_str());
00167 if (!fin.good()) {
00168 ROS_ERROR("Unable to open camera calibration file [%s]", file_name.c_str());
00169 return false;
00170 }
00171 bool success = readCalibrationYml(fin, camera_name, cam_info);
00172 if (!success)
00173 ROS_ERROR("Failed to parse camera calibration from file [%s]", file_name.c_str());
00174 return success;
00175 }
00176
00177 }