parse_ini.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 <ros/console.h>
38 
39 #include <boost/spirit/include/phoenix_stl.hpp>
40 #include <boost/spirit/include/qi.hpp>
41 #include <boost/spirit/include/classic_core.hpp>
42 #include <boost/spirit/include/classic_file_iterator.hpp>
43 #include <boost/spirit/include/classic_confix.hpp>
44 #include <boost/spirit/include/classic_loops.hpp>
45 #include <boost/typeof/typeof.hpp>
46 #include <boost/filesystem.hpp>
47 #include <iterator>
48 #include <fstream>
49 
51 
53 using namespace BOOST_SPIRIT_CLASSIC_NS;
54 
56 
57 struct SimpleMatrix
58 {
59  int rows;
60  int cols;
61  const double* data;
62 
63  SimpleMatrix(int rows, int cols, const double* data)
64  : rows(rows), cols(cols), data(data)
65  {}
66 };
67 
68 std::ostream& operator << (std::ostream& out, const SimpleMatrix& m)
69 {
70  for (int i = 0; i < m.rows; ++i) {
71  for (int j = 0; j < m.cols; ++j) {
72  out << m.data[m.cols*i+j] << " ";
73  }
74  out << std::endl;
75  }
76  return out;
77 }
78 
80 
81 bool writeCalibrationIni(std::ostream& out, const std::string& camera_name,
82  const sensor_msgs::CameraInfo& cam_info)
83 {
84  // Videre INI format is legacy, only supports plumb bob distortion model.
85  if (cam_info.distortion_model != sensor_msgs::distortion_models::PLUMB_BOB ||
86  cam_info.D.size() != 5) {
87  ROS_ERROR("Videre INI format can only save calibrations using the plumb bob distortion model. "
88  "Use the YAML format instead.\n"
89  "\tdistortion_model = '%s', expected '%s'\n"
90  "\tD.size() = %d, expected 5", cam_info.distortion_model.c_str(),
91  sensor_msgs::distortion_models::PLUMB_BOB.c_str(), (int)cam_info.D.size());
92  return false;
93  }
94 
95  out.precision(5);
96  out << std::fixed;
97 
98  out << "# Camera intrinsics\n\n";
100  out << "[image]\n\n";
101  out << "width\n" << cam_info.width << "\n\n";
102  out << "height\n" << cam_info.height << "\n\n";
103  out << "[" << camera_name << "]\n\n";
104 
105  out << "camera matrix\n" << SimpleMatrix(3, 3, &cam_info.K[0]);
106  out << "\ndistortion\n" << SimpleMatrix(1, 5, &cam_info.D[0]);
107  out << "\n\nrectification\n" << SimpleMatrix(3, 3, &cam_info.R[0]);
108  out << "\nprojection\n" << SimpleMatrix(3, 4, &cam_info.P[0]);
109 
110  return true;
111 }
112 
113 bool writeCalibrationIni(const std::string& file_name, const std::string& camera_name,
114  const sensor_msgs::CameraInfo& cam_info)
115 {
116  boost::filesystem::path dir(boost::filesystem::path(file_name).parent_path());
117  if (!dir.empty() && !boost::filesystem::exists(dir) &&
118  !boost::filesystem::create_directories(dir)){
119  ROS_ERROR("Unable to create directory for camera calibration file [%s]", dir.c_str());
120  }
121  std::ofstream out(file_name.c_str());
122  if (!out.is_open())
123  {
124  ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str());
125  return false;
126  }
127  return writeCalibrationIni(out, camera_name, cam_info);
128 }
129 
131 // Semantic action to store a sequence of values in an array
132 template <typename T>
133 struct ArrayAssignActor
134 {
135  ArrayAssignActor(T* start)
136  : ptr_(start)
137  {}
138 
139  void operator()(T val) const
140  {
141  *ptr_++ = val;
142  }
143 
144  mutable T* ptr_;
145 };
146 
147 // Semantic action generator
148 template <typename T>
149 ArrayAssignActor<T> array_assign_a(T* start)
150 {
151  return ArrayAssignActor<T>(start);
152 }
153 
154 template <typename Iterator>
155 bool parseCalibrationIniRange(Iterator first, Iterator last,
156  std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
157 {
158  cam_info.D.clear();
159 
160  // We don't actually use the [externals] info, but it's part of the format
161  bool have_externals = false;
162  double trans[3], rot[3];
163 
165 
166  // Image section (width, height)
167  BOOST_AUTO(image,
168  str_p("[image]")
169  >> "width"
170  >> uint_p[assign_a(cam_info.width)]
171  >> "height"
172  >> uint_p[assign_a(cam_info.height)]
173  );
174 
175  // Optional externals section
176  BOOST_AUTO(externals,
177  str_p("[externals]")
178  >> "translation"
179  >> repeat_p(3)[real_p[array_assign_a(trans)]]
180  >> "rotation"
181  >> repeat_p(3)[real_p[array_assign_a(rot)]]
182  );
183 
184  // Parser to save name of camera section
185  BOOST_AUTO(name, confix_p('[', (*anychar_p)[assign_a(camera_name)], ']'));
186 
187  // Camera section (intrinsics)
188  BOOST_AUTO(camera,
189  name
190  >> "camera matrix"
191  >> repeat_p(9)[real_p[array_assign_a(&cam_info.K[0])]]
192  >> "distortion"
193  >> *(real_p[push_back_a(cam_info.D)])
194  >> "rectification"
195  >> repeat_p(9)[real_p[array_assign_a(&cam_info.R[0])]]
196  >> "projection"
197  >> repeat_p(12)[real_p[array_assign_a(&cam_info.P[0])]]
198  );
199 
200  // Full grammar
201  BOOST_AUTO(ini_grammar,
202  image
203  >> !externals[assign_a(have_externals, true)]
204  >> camera);
205 
206  // Skip whitespace and line comments
207  BOOST_AUTO(skip, space_p | comment_p('#'));
208 
209  parse_info<Iterator> info = parse(first, last, ini_grammar, skip);
210 
211  // Figure out the distortion model
212  if (cam_info.D.size() == 5)
213  cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
214  else if (cam_info.D.size() == 8)
215  cam_info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
216 
217  return info.hit;
218 }
220 
221 bool readCalibrationIni(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
222 {
223  std::istream_iterator<char> first(in), last;
224  return parseCalibrationIniRange(first, last, camera_name, cam_info);
225 }
226 
227 bool readCalibrationIni(const std::string& file_name, std::string& camera_name,
228  sensor_msgs::CameraInfo& cam_info)
229 {
230  typedef file_iterator<char> Iterator;
231 
232  Iterator first(file_name);
233  if (!first) {
234  ROS_INFO("Unable to open camera calibration file [%s]", file_name.c_str());
235  return false;
236  }
237  Iterator last = first.make_end();
238 
239  return parseCalibrationIniRange(first, last, camera_name, cam_info);
240 }
241 
242 bool parseCalibrationIni(const std::string& buffer, std::string& camera_name,
243  sensor_msgs::CameraInfo& cam_info)
244 {
245  return parseCalibrationIniRange(buffer.begin(), buffer.end(), camera_name, cam_info);
246 }
247 
248 } //namespace camera_calibration_parsers
const std::string RATIONAL_POLYNOMIAL
ROSCPP_DECL void start()
#define ROS_INFO(...)
bool writeCalibrationIni(std::ostream &out, const std::string &camera_name, const sensor_msgs::CameraInfo &cam_info)
Write calibration parameters to a file in INI format.
Definition: parse_ini.cpp:81
bool parseCalibrationIni(const std::string &buffer, std::string &camera_name, sensor_msgs::CameraInfo &cam_info)
Parse calibration parameters from a string in memory of INI format.
Definition: parse_ini.cpp:242
#define ROS_ERROR(...)
bool readCalibrationIni(std::istream &in, std::string &camera_name, sensor_msgs::CameraInfo &cam_info)
Read calibration parameters from an INI file.
Definition: parse_ini.cpp:221


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