parse_yml.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include "camera_calibration_parsers/parse_yml.h"
00036 #include <sensor_msgs/distortion_models.h>
00037 #include <boost/filesystem.hpp>
00038 #include <yaml-cpp/yaml.h>
00039 #include <fstream>
00040 #include <ctime>
00041 #include <cassert>
00042 #include <cstring>
00043 #include <ros/console.h>
00044 
00045 namespace camera_calibration_parsers {
00046 
00048 
00049 static const char CAM_YML_NAME[]    = "camera_name";
00050 static const char WIDTH_YML_NAME[]  = "image_width";
00051 static const char HEIGHT_YML_NAME[] = "image_height";
00052 static const char K_YML_NAME[]      = "camera_matrix";
00053 static const char D_YML_NAME[]      = "distortion_coefficients";
00054 static const char R_YML_NAME[]      = "rectification_matrix";
00055 static const char P_YML_NAME[]      = "projection_matrix";
00056 static const char DMODEL_YML_NAME[] = "distortion_model";
00057 
00058 struct SimpleMatrix
00059 {
00060   int rows;
00061   int cols;
00062   double* data;
00063 
00064   SimpleMatrix(int rows, int cols, double* data)
00065     : rows(rows), cols(cols), data(data)
00066   {}
00067 };
00068 
00069 YAML::Emitter& operator << (YAML::Emitter& out, const SimpleMatrix& m)
00070 {
00071   out << YAML::BeginMap;
00072   out << YAML::Key << "rows" << YAML::Value << m.rows;
00073   out << YAML::Key << "cols" << YAML::Value << m.cols;
00074   //out << YAML::Key << "dt"   << YAML::Value << "d"; // OpenCV data type specifier
00075   out << YAML::Key << "data" << YAML::Value;
00076   out << YAML::Flow;
00077   out << YAML::BeginSeq;
00078   for (int i = 0; i < m.rows*m.cols; ++i)
00079     out << m.data[i];
00080   out << YAML::EndSeq;
00081   out << YAML::EndMap;
00082   return out;
00083 }
00084 
00085 #ifdef HAVE_NEW_YAMLCPP
00086 template<typename T>
00087 void operator >> (const YAML::Node& node, T& i)
00088 {
00089   i = node.as<T>();
00090 }
00091 #endif
00092 
00093 void operator >> (const YAML::Node& node, SimpleMatrix& m)
00094 {
00095   int rows, cols;
00096   node["rows"] >> rows;
00097   assert(rows == m.rows);
00098   node["cols"] >> cols;
00099   assert(cols == m.cols);
00100   const YAML::Node& data = node["data"];
00101   for (int i = 0; i < rows*cols; ++i)
00102     data[i] >> m.data[i];
00103 }
00104 
00106 
00107 bool writeCalibrationYml(std::ostream& out, const std::string& camera_name,
00108                          const sensor_msgs::CameraInfo& cam_info)
00109 {
00110   YAML::Emitter emitter;
00111   emitter << YAML::BeginMap;
00112 
00113 #if 0
00114   // Calibration time
00116   time_t raw_time;
00117   time( &raw_time );
00118   emitter << YAML::Key << "calibration_time";
00119   emitter << YAML::Value << asctime(localtime(&raw_time));
00120 #endif
00121 
00122   // Image dimensions
00123   emitter << YAML::Key << WIDTH_YML_NAME << YAML::Value << (int)cam_info.width;
00124   emitter << YAML::Key << HEIGHT_YML_NAME << YAML::Value << (int)cam_info.height;
00125   
00126   // Camera name and intrinsics
00127   emitter << YAML::Key << CAM_YML_NAME << YAML::Value << camera_name;
00128   emitter << YAML::Key << K_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.K[0]));
00129   emitter << YAML::Key << DMODEL_YML_NAME << YAML::Value << cam_info.distortion_model;
00130   emitter << YAML::Key << D_YML_NAME << YAML::Value << SimpleMatrix(1, cam_info.D.size(),
00131                                                                     const_cast<double*>(&cam_info.D[0]));
00132   emitter << YAML::Key << R_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.R[0]));
00133   emitter << YAML::Key << P_YML_NAME << YAML::Value << SimpleMatrix(3, 4, const_cast<double*>(&cam_info.P[0]));
00134 
00135   emitter << YAML::EndMap;
00136 
00137   out << emitter.c_str();
00138   return true;
00139 }
00140 
00141 bool writeCalibrationYml(const std::string& file_name, const std::string& camera_name,
00142                          const sensor_msgs::CameraInfo& cam_info)
00143 {
00144   boost::filesystem::path dir(boost::filesystem::path(file_name).parent_path());
00145   if (!dir.empty() && !boost::filesystem::exists(dir) &&
00146      !boost::filesystem::create_directories(dir)){
00147     ROS_ERROR("Unable to create directory for camera calibration file [%s]", dir.c_str());
00148   }
00149   std::ofstream out(file_name.c_str());
00150   if (!out.is_open())
00151   {
00152     ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str());
00153     return false;
00154   }
00155   return writeCalibrationYml(out, camera_name, cam_info);
00156 }
00157 
00158 bool readCalibrationYml(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
00159 {
00160   try {
00161 #ifdef HAVE_NEW_YAMLCPP
00162     YAML::Node doc = YAML::Load(in);
00163 
00164     if (doc[CAM_YML_NAME])
00165       doc[CAM_YML_NAME] >> camera_name;
00166     else
00167       camera_name = "unknown";
00168 #else
00169     YAML::Parser parser(in);
00170     if (!parser) {
00171       ROS_ERROR("Unable to create YAML parser for camera calibration");
00172       return false;
00173     }
00174     YAML::Node doc;
00175     parser.GetNextDocument(doc);
00176 
00177     if (const YAML::Node* name_node = doc.FindValue(CAM_YML_NAME))
00178       *name_node >> camera_name;
00179     else
00180       camera_name = "unknown";
00181 #endif
00182 
00183     doc[WIDTH_YML_NAME] >> cam_info.width;
00184     doc[HEIGHT_YML_NAME] >> cam_info.height;
00185 
00186     // Read in fixed-size matrices
00187     SimpleMatrix K_(3, 3, &cam_info.K[0]);
00188     doc[K_YML_NAME] >> K_;
00189     SimpleMatrix R_(3, 3, &cam_info.R[0]);
00190     doc[R_YML_NAME] >> R_;
00191     SimpleMatrix P_(3, 4, &cam_info.P[0]);
00192     doc[P_YML_NAME] >> P_;
00193 
00194     // Different distortion models may have different numbers of parameters
00195 #ifdef HAVE_NEW_YAMLCPP
00196     if (doc[DMODEL_YML_NAME]) {
00197       doc[DMODEL_YML_NAME] >> cam_info.distortion_model;
00198     }
00199 #else
00200     if (const YAML::Node* model_node = doc.FindValue(DMODEL_YML_NAME)) {
00201       *model_node >> cam_info.distortion_model;
00202     }
00203 #endif
00204     else {
00205       // Assume plumb bob for backwards compatibility
00206       cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00207       ROS_WARN("Camera calibration file did not specify distortion model, assuming plumb bob");
00208     }
00209     const YAML::Node& D_node = doc[D_YML_NAME];
00210     int D_rows, D_cols;
00211     D_node["rows"] >> D_rows;
00212     D_node["cols"] >> D_cols;
00213     const YAML::Node& D_data = D_node["data"];
00214     cam_info.D.resize(D_rows*D_cols);
00215     for (int i = 0; i < D_rows*D_cols; ++i)
00216       D_data[i] >> cam_info.D[i];
00217   
00218     return true;
00219   }
00220   catch (YAML::Exception& e) {
00221     ROS_ERROR("Exception parsing YAML camera calibration:\n%s", e.what());
00222     return false;
00223   }
00224 }
00225 
00226 bool readCalibrationYml(const std::string& file_name, std::string& camera_name,
00227                         sensor_msgs::CameraInfo& cam_info)
00228 {
00229   std::ifstream fin(file_name.c_str());
00230   if (!fin.good()) {
00231     ROS_INFO("Unable to open camera calibration file [%s]", file_name.c_str());
00232     return false;
00233   }
00234   bool success = readCalibrationYml(fin, camera_name, cam_info);
00235   if (!success)
00236     ROS_ERROR("Failed to parse camera calibration from file [%s]", file_name.c_str());
00237   return success;
00238 }
00239 
00240 } //namespace camera_calibration_parsers


camera_calibration_parsers
Author(s): Patrick Mihelich
autogenerated on Thu Aug 27 2015 13:30:13