parse_ini.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_ini.h"
00036 #include <sensor_msgs/distortion_models.h>
00037 #include <ros/console.h>
00038 
00039 #include <boost/spirit/include/classic_core.hpp>
00040 #include <boost/spirit/include/classic_file_iterator.hpp>
00041 #include <boost/spirit/include/classic_confix.hpp>
00042 #include <boost/spirit/include/classic_loops.hpp>
00043 #include <boost/typeof/typeof.hpp>
00044 #include <iterator>
00045 #include <fstream>
00046 
00047 namespace camera_calibration_parsers {
00048 
00050 using namespace BOOST_SPIRIT_CLASSIC_NS;
00051 
00053 
00054 struct SimpleMatrix
00055 {
00056   int rows;
00057   int cols;
00058   const double* data;
00059 
00060   SimpleMatrix(int rows, int cols, const double* data)
00061     : rows(rows), cols(cols), data(data)
00062   {}
00063 };
00064 
00065 std::ostream& operator << (std::ostream& out, const SimpleMatrix& m)
00066 {
00067   for (int i = 0; i < m.rows; ++i) {
00068     for (int j = 0; j < m.cols; ++j) {
00069       out << m.data[m.cols*i+j] << " ";
00070     }
00071     out << std::endl;
00072   }
00073   return out;
00074 }
00075 
00077 
00078 bool writeCalibrationIni(std::ostream& out, const std::string& camera_name,
00079                          const sensor_msgs::CameraInfo& cam_info)
00080 {
00081   // Videre INI format is legacy, only supports plumb bob distortion model.
00082   if (cam_info.distortion_model != sensor_msgs::distortion_models::PLUMB_BOB ||
00083       cam_info.D.size() != 5) {
00084     ROS_ERROR("Videre INI format can only save calibrations using the plumb bob distortion model. "
00085               "Use the YAML format instead.\n"
00086               "\tdistortion_model = '%s', expected '%s'\n"
00087               "\tD.size() = %d, expected 5", cam_info.distortion_model.c_str(),
00088               sensor_msgs::distortion_models::PLUMB_BOB.c_str(), (int)cam_info.D.size());
00089     return false;
00090   }
00091   
00092   out.precision(5);
00093   out << std::fixed;
00094   
00095   out << "# Camera intrinsics\n\n";
00097   out << "[image]\n\n";
00098   out << "width\n" << cam_info.width << "\n\n";
00099   out << "height\n" << cam_info.height << "\n\n";
00100   out << "[" << camera_name << "]\n\n";
00101 
00102   out << "camera matrix\n"     << SimpleMatrix(3, 3, &cam_info.K[0]);
00103   out << "\ndistortion\n"      << SimpleMatrix(1, 5, &cam_info.D[0]);
00104   out << "\n\nrectification\n" << SimpleMatrix(3, 3, &cam_info.R[0]);
00105   out << "\nprojection\n"      << SimpleMatrix(3, 4, &cam_info.P[0]);
00106 
00107   return true;
00108 }
00109 
00110 bool writeCalibrationIni(const std::string& file_name, const std::string& camera_name,
00111                          const sensor_msgs::CameraInfo& cam_info)
00112 {
00113   std::ofstream out(file_name.c_str());
00114   if (!out.is_open())
00115   {
00116     ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str());
00117     return false;
00118   }
00119   return writeCalibrationIni(out, camera_name, cam_info);
00120 }
00121 
00123 // Semantic action to store a sequence of values in an array
00124 template <typename T>
00125 struct ArrayAssignActor
00126 {
00127   ArrayAssignActor(T* start)
00128     : ptr_(start)
00129   {}
00130 
00131   void operator()(T val) const
00132   {
00133     *ptr_++ = val;
00134   }
00135 
00136   mutable T* ptr_;
00137 };
00138 
00139 // Semantic action generator
00140 template <typename T>
00141 ArrayAssignActor<T> array_assign_a(T* start)
00142 {
00143   return ArrayAssignActor<T>(start);
00144 }
00145 
00146 template <typename Iterator>
00147 bool parseCalibrationIniRange(Iterator first, Iterator last,
00148                               std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
00149 {
00150   // Assume plumb bob model
00151   cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00152   cam_info.D.resize(5);
00153   
00154   // We don't actually use the [externals] info, but it's part of the format
00155   bool have_externals = false;
00156   double trans[3], rot[3];
00157 
00159   
00160   // Image section (width, height)
00161   BOOST_AUTO(image,
00162       str_p("[image]")
00163       >> "width"
00164       >> uint_p[assign_a(cam_info.width)]
00165       >> "height"
00166       >> uint_p[assign_a(cam_info.height)]
00167      );
00168 
00169   // Optional externals section
00170   BOOST_AUTO(externals,
00171       str_p("[externals]")
00172       >> "translation"
00173       >> repeat_p(3)[real_p[array_assign_a(trans)]]
00174       >> "rotation"
00175       >> repeat_p(3)[real_p[array_assign_a(rot)]]
00176      );
00177 
00178   // Parser to save name of camera section
00179   BOOST_AUTO(name, confix_p('[', (*anychar_p)[assign_a(camera_name)], ']'));
00180 
00181   // Camera section (intrinsics)
00182   BOOST_AUTO(camera,
00183       name
00184       >> "camera matrix"
00185       >> repeat_p(9)[real_p[array_assign_a(&cam_info.K[0])]]
00186       >> "distortion"
00187       >> repeat_p(5)[real_p[array_assign_a(&cam_info.D[0])]]
00188       >> "rectification"
00189       >> repeat_p(9)[real_p[array_assign_a(&cam_info.R[0])]]
00190       >> "projection"
00191       >> repeat_p(12)[real_p[array_assign_a(&cam_info.P[0])]]
00192      );
00193 
00194   // Full grammar
00195   BOOST_AUTO(ini_grammar,
00196       image
00197       >> !externals[assign_a(have_externals, true)]
00198       >>  camera);
00199 
00200   // Skip whitespace and line comments
00201   BOOST_AUTO(skip, space_p | comment_p('#'));
00202 
00203   parse_info<Iterator> info = parse(first, last, ini_grammar, skip);
00204   return info.hit;
00205 }
00207 
00208 bool readCalibrationIni(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
00209 {
00210   std::istream_iterator<char> first(in), last;
00211   return parseCalibrationIniRange(first, last, camera_name, cam_info);
00212 }
00213 
00214 bool readCalibrationIni(const std::string& file_name, std::string& camera_name,
00215                         sensor_msgs::CameraInfo& cam_info)
00216 {
00217   typedef file_iterator<char> Iterator;
00218 
00219   Iterator first(file_name);
00220   if (!first) {
00221     ROS_ERROR("Unable to open camera calibration file [%s]", file_name.c_str());
00222     return false;
00223   }
00224   Iterator last = first.make_end();
00225 
00226   return parseCalibrationIniRange(first, last, camera_name, cam_info);
00227 }
00228 
00229 bool parseCalibrationIni(const std::string& buffer, std::string& camera_name,
00230                          sensor_msgs::CameraInfo& cam_info)
00231 {
00232   return parseCalibrationIniRange(buffer.begin(), buffer.end(), camera_name, cam_info);
00233 }
00234 
00235 } //namespace camera_calibration_parsers


camera_calibration_parsers
Author(s): Patrick Mihelich
autogenerated on Mon Oct 6 2014 00:42:35