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


camera_calibration_parsers
Author(s): Patrick Mihelich
autogenerated on Fri Jan 3 2014 11:24:03