parse_yml.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
37 #include <boost/filesystem.hpp>
38 #include <yaml-cpp/yaml.h>
39 #include <fstream>
40 #include <ctime>
41 #include <cassert>
42 #include <cstring>
43 #include <ros/console.h>
44 
46 
48 
49 static const char CAM_YML_NAME[] = "camera_name";
50 static const char WIDTH_YML_NAME[] = "image_width";
51 static const char HEIGHT_YML_NAME[] = "image_height";
52 static const char K_YML_NAME[] = "camera_matrix";
53 static const char D_YML_NAME[] = "distortion_coefficients";
54 static const char R_YML_NAME[] = "rectification_matrix";
55 static const char P_YML_NAME[] = "projection_matrix";
56 static const char DMODEL_YML_NAME[] = "distortion_model";
57 
58 struct SimpleMatrix
59 {
60  int rows;
61  int cols;
62  double* data;
63 
64  SimpleMatrix(int rows, int cols, double* data)
65  : rows(rows), cols(cols), data(data)
66  {}
67 };
68 
69 YAML::Emitter& operator << (YAML::Emitter& out, const SimpleMatrix& m)
70 {
71  out << YAML::BeginMap;
72  out << YAML::Key << "rows" << YAML::Value << m.rows;
73  out << YAML::Key << "cols" << YAML::Value << m.cols;
74  //out << YAML::Key << "dt" << YAML::Value << "d"; // OpenCV data type specifier
75  out << YAML::Key << "data" << YAML::Value;
76  out << YAML::Flow;
77  out << YAML::BeginSeq;
78  for (int i = 0; i < m.rows*m.cols; ++i)
79  out << m.data[i];
80  out << YAML::EndSeq;
81  out << YAML::EndMap;
82  return out;
83 }
84 
85 #ifdef HAVE_NEW_YAMLCPP
86 template<typename T>
87 void operator >> (const YAML::Node& node, T& i)
88 {
89  i = node.as<T>();
90 }
91 #endif
92 
93 void operator >> (const YAML::Node& node, SimpleMatrix& m)
94 {
95  int rows, cols;
96  node["rows"] >> rows;
97  assert(rows == m.rows);
98  node["cols"] >> cols;
99  assert(cols == m.cols);
100  const YAML::Node& data = node["data"];
101  for (int i = 0; i < rows*cols; ++i)
102  data[i] >> m.data[i];
103 }
104 
106 
107 bool writeCalibrationYml(std::ostream& out, const std::string& camera_name,
108  const sensor_msgs::CameraInfo& cam_info)
109 {
110  YAML::Emitter emitter;
111  emitter << YAML::BeginMap;
112 
113 #if 0
114  // Calibration time
116  time_t raw_time;
117  time( &raw_time );
118  emitter << YAML::Key << "calibration_time";
119  emitter << YAML::Value << asctime(localtime(&raw_time));
120 #endif
121 
122  // Image dimensions
123  emitter << YAML::Key << WIDTH_YML_NAME << YAML::Value << (int)cam_info.width;
124  emitter << YAML::Key << HEIGHT_YML_NAME << YAML::Value << (int)cam_info.height;
125 
126  // Camera name and intrinsics
127  emitter << YAML::Key << CAM_YML_NAME << YAML::Value << camera_name;
128  emitter << YAML::Key << K_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.K[0]));
129  emitter << YAML::Key << DMODEL_YML_NAME << YAML::Value << cam_info.distortion_model;
130  emitter << YAML::Key << D_YML_NAME << YAML::Value << SimpleMatrix(1, cam_info.D.size(),
131  const_cast<double*>(&cam_info.D[0]));
132  emitter << YAML::Key << R_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.R[0]));
133  emitter << YAML::Key << P_YML_NAME << YAML::Value << SimpleMatrix(3, 4, const_cast<double*>(&cam_info.P[0]));
134 
135  emitter << YAML::EndMap;
136 
137  out << emitter.c_str();
138  return true;
139 }
140 
141 bool writeCalibrationYml(const std::string& file_name, const std::string& camera_name,
142  const sensor_msgs::CameraInfo& cam_info)
143 {
144  boost::filesystem::path dir(boost::filesystem::path(file_name).parent_path());
145  if (!dir.empty() && !boost::filesystem::exists(dir) &&
146  !boost::filesystem::create_directories(dir)){
147  ROS_ERROR("Unable to create directory for camera calibration file [%s]", dir.c_str());
148  }
149  std::ofstream out(file_name.c_str());
150  if (!out.is_open())
151  {
152  ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str());
153  return false;
154  }
155  return writeCalibrationYml(out, camera_name, cam_info);
156 }
157 
158 bool readCalibrationYml(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
159 {
160  try {
161 #ifdef HAVE_NEW_YAMLCPP
162  YAML::Node doc = YAML::Load(in);
163 
164  if (doc[CAM_YML_NAME])
165  doc[CAM_YML_NAME] >> camera_name;
166  else
167  camera_name = "unknown";
168 #else
169  YAML::Parser parser(in);
170  if (!parser) {
171  ROS_ERROR("Unable to create YAML parser for camera calibration");
172  return false;
173  }
174  YAML::Node doc;
175  parser.GetNextDocument(doc);
176 
177  if (const YAML::Node* name_node = doc.FindValue(CAM_YML_NAME))
178  *name_node >> camera_name;
179  else
180  camera_name = "unknown";
181 #endif
182 
183  doc[WIDTH_YML_NAME] >> cam_info.width;
184  doc[HEIGHT_YML_NAME] >> cam_info.height;
185 
186  // Read in fixed-size matrices
187  SimpleMatrix K_(3, 3, &cam_info.K[0]);
188  doc[K_YML_NAME] >> K_;
189  SimpleMatrix R_(3, 3, &cam_info.R[0]);
190  doc[R_YML_NAME] >> R_;
191  SimpleMatrix P_(3, 4, &cam_info.P[0]);
192  doc[P_YML_NAME] >> P_;
193 
194  // Different distortion models may have different numbers of parameters
195 #ifdef HAVE_NEW_YAMLCPP
196  if (doc[DMODEL_YML_NAME]) {
197  doc[DMODEL_YML_NAME] >> cam_info.distortion_model;
198  }
199 #else
200  if (const YAML::Node* model_node = doc.FindValue(DMODEL_YML_NAME)) {
201  *model_node >> cam_info.distortion_model;
202  }
203 #endif
204  else {
205  // Assume plumb bob for backwards compatibility
206  cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
207  ROS_WARN("Camera calibration file did not specify distortion model, assuming plumb bob");
208  }
209  const YAML::Node& D_node = doc[D_YML_NAME];
210  int D_rows, D_cols;
211  D_node["rows"] >> D_rows;
212  D_node["cols"] >> D_cols;
213  const YAML::Node& D_data = D_node["data"];
214  cam_info.D.resize(D_rows*D_cols);
215  for (int i = 0; i < D_rows*D_cols; ++i)
216  D_data[i] >> cam_info.D[i];
217 
218  return true;
219  }
220  catch (YAML::Exception& e) {
221  ROS_ERROR("Exception parsing YAML camera calibration:\n%s", e.what());
222  return false;
223  }
224 }
225 
226 bool readCalibrationYml(const std::string& file_name, std::string& camera_name,
227  sensor_msgs::CameraInfo& cam_info)
228 {
229  std::ifstream fin(file_name.c_str());
230  if (!fin.good()) {
231  ROS_INFO("Unable to open camera calibration file [%s]", file_name.c_str());
232  return false;
233  }
234  bool success = readCalibrationYml(fin, camera_name, cam_info);
235  if (!success)
236  ROS_ERROR("Failed to parse camera calibration from file [%s]", file_name.c_str());
237  return success;
238 }
239 
240 } //namespace camera_calibration_parsers
bool readCalibrationYml(std::istream &in, std::string &camera_name, sensor_msgs::CameraInfo &cam_info)
Read calibration parameters from a YAML file.
Definition: parse_yml.cpp:158
#define ROS_WARN(...)
#define ROS_INFO(...)
Definition: parser.py:1
bool writeCalibrationYml(std::ostream &out, const std::string &camera_name, const sensor_msgs::CameraInfo &cam_info)
Write calibration parameters to a file in YAML format.
Definition: parse_yml.cpp:107
#define ROS_ERROR(...)


camera_calibration_parsers
Author(s): Patrick Mihelich
autogenerated on Mon Feb 28 2022 22:31:43