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


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