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
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
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
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
00117 cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00118 cam_info.D.resize(5);
00119
00120
00121 bool have_externals = false;
00122 double trans[3], rot[3];
00123
00125
00126
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
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
00145 BOOST_AUTO(name, confix_p('[', (*anychar_p)[assign_a(camera_name)], ']'));
00146
00147
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
00161 BOOST_AUTO(ini_grammar,
00162 image
00163 >> !externals[assign_a(have_externals, true)]
00164 >> camera);
00165
00166
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 }