Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "camera_calibration_parsers/parse_yml.h"
00036 #include <sensor_msgs/distortion_models.h>
00037 #include <boost/filesystem.hpp>
00038 #include <yaml-cpp/yaml.h>
00039 #include <fstream>
00040 #include <ctime>
00041 #include <cassert>
00042 #include <cstring>
00043 #include <ros/console.h>
00044
00045 namespace camera_calibration_parsers {
00046
00048
00049 static const char CAM_YML_NAME[] = "camera_name";
00050 static const char WIDTH_YML_NAME[] = "image_width";
00051 static const char HEIGHT_YML_NAME[] = "image_height";
00052 static const char K_YML_NAME[] = "camera_matrix";
00053 static const char D_YML_NAME[] = "distortion_coefficients";
00054 static const char R_YML_NAME[] = "rectification_matrix";
00055 static const char P_YML_NAME[] = "projection_matrix";
00056 static const char DMODEL_YML_NAME[] = "distortion_model";
00057
00058 struct SimpleMatrix
00059 {
00060 int rows;
00061 int cols;
00062 double* data;
00063
00064 SimpleMatrix(int rows, int cols, double* data)
00065 : rows(rows), cols(cols), data(data)
00066 {}
00067 };
00068
00069 YAML::Emitter& operator << (YAML::Emitter& out, const SimpleMatrix& m)
00070 {
00071 out << YAML::BeginMap;
00072 out << YAML::Key << "rows" << YAML::Value << m.rows;
00073 out << YAML::Key << "cols" << YAML::Value << m.cols;
00074
00075 out << YAML::Key << "data" << YAML::Value;
00076 out << YAML::Flow;
00077 out << YAML::BeginSeq;
00078 for (int i = 0; i < m.rows*m.cols; ++i)
00079 out << m.data[i];
00080 out << YAML::EndSeq;
00081 out << YAML::EndMap;
00082 return out;
00083 }
00084
00085 #ifdef HAVE_NEW_YAMLCPP
00086 template<typename T>
00087 void operator >> (const YAML::Node& node, T& i)
00088 {
00089 i = node.as<T>();
00090 }
00091 #endif
00092
00093 void operator >> (const YAML::Node& node, SimpleMatrix& m)
00094 {
00095 int rows, cols;
00096 node["rows"] >> rows;
00097 assert(rows == m.rows);
00098 node["cols"] >> cols;
00099 assert(cols == m.cols);
00100 const YAML::Node& data = node["data"];
00101 for (int i = 0; i < rows*cols; ++i)
00102 data[i] >> m.data[i];
00103 }
00104
00106
00107 bool writeCalibrationYml(std::ostream& out, const std::string& camera_name,
00108 const sensor_msgs::CameraInfo& cam_info)
00109 {
00110 YAML::Emitter emitter;
00111 emitter << YAML::BeginMap;
00112
00113 #if 0
00114
00116 time_t raw_time;
00117 time( &raw_time );
00118 emitter << YAML::Key << "calibration_time";
00119 emitter << YAML::Value << asctime(localtime(&raw_time));
00120 #endif
00121
00122
00123 emitter << YAML::Key << WIDTH_YML_NAME << YAML::Value << (int)cam_info.width;
00124 emitter << YAML::Key << HEIGHT_YML_NAME << YAML::Value << (int)cam_info.height;
00125
00126
00127 emitter << YAML::Key << CAM_YML_NAME << YAML::Value << camera_name;
00128 emitter << YAML::Key << K_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.K[0]));
00129 emitter << YAML::Key << DMODEL_YML_NAME << YAML::Value << cam_info.distortion_model;
00130 emitter << YAML::Key << D_YML_NAME << YAML::Value << SimpleMatrix(1, cam_info.D.size(),
00131 const_cast<double*>(&cam_info.D[0]));
00132 emitter << YAML::Key << R_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.R[0]));
00133 emitter << YAML::Key << P_YML_NAME << YAML::Value << SimpleMatrix(3, 4, const_cast<double*>(&cam_info.P[0]));
00134
00135 emitter << YAML::EndMap;
00136
00137 out << emitter.c_str();
00138 return true;
00139 }
00140
00141 bool writeCalibrationYml(const std::string& file_name, const std::string& camera_name,
00142 const sensor_msgs::CameraInfo& cam_info)
00143 {
00144 boost::filesystem::path dir(boost::filesystem::path(file_name).parent_path());
00145 if (!dir.empty() && !boost::filesystem::exists(dir) &&
00146 !boost::filesystem::create_directories(dir)){
00147 ROS_ERROR("Unable to create directory for camera calibration file [%s]", dir.c_str());
00148 }
00149 std::ofstream out(file_name.c_str());
00150 if (!out.is_open())
00151 {
00152 ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str());
00153 return false;
00154 }
00155 return writeCalibrationYml(out, camera_name, cam_info);
00156 }
00157
00158 bool readCalibrationYml(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
00159 {
00160 try {
00161 #ifdef HAVE_NEW_YAMLCPP
00162 YAML::Node doc = YAML::Load(in);
00163
00164 if (doc[CAM_YML_NAME])
00165 doc[CAM_YML_NAME] >> camera_name;
00166 else
00167 camera_name = "unknown";
00168 #else
00169 YAML::Parser parser(in);
00170 if (!parser) {
00171 ROS_ERROR("Unable to create YAML parser for camera calibration");
00172 return false;
00173 }
00174 YAML::Node doc;
00175 parser.GetNextDocument(doc);
00176
00177 if (const YAML::Node* name_node = doc.FindValue(CAM_YML_NAME))
00178 *name_node >> camera_name;
00179 else
00180 camera_name = "unknown";
00181 #endif
00182
00183 doc[WIDTH_YML_NAME] >> cam_info.width;
00184 doc[HEIGHT_YML_NAME] >> cam_info.height;
00185
00186
00187 SimpleMatrix K_(3, 3, &cam_info.K[0]);
00188 doc[K_YML_NAME] >> K_;
00189 SimpleMatrix R_(3, 3, &cam_info.R[0]);
00190 doc[R_YML_NAME] >> R_;
00191 SimpleMatrix P_(3, 4, &cam_info.P[0]);
00192 doc[P_YML_NAME] >> P_;
00193
00194
00195 #ifdef HAVE_NEW_YAMLCPP
00196 if (doc[DMODEL_YML_NAME]) {
00197 doc[DMODEL_YML_NAME] >> cam_info.distortion_model;
00198 }
00199 #else
00200 if (const YAML::Node* model_node = doc.FindValue(DMODEL_YML_NAME)) {
00201 *model_node >> cam_info.distortion_model;
00202 }
00203 #endif
00204 else {
00205
00206 cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00207 ROS_WARN("Camera calibration file did not specify distortion model, assuming plumb bob");
00208 }
00209 const YAML::Node& D_node = doc[D_YML_NAME];
00210 int D_rows, D_cols;
00211 D_node["rows"] >> D_rows;
00212 D_node["cols"] >> D_cols;
00213 const YAML::Node& D_data = D_node["data"];
00214 cam_info.D.resize(D_rows*D_cols);
00215 for (int i = 0; i < D_rows*D_cols; ++i)
00216 D_data[i] >> cam_info.D[i];
00217
00218 return true;
00219 }
00220 catch (YAML::Exception& e) {
00221 ROS_ERROR("Exception parsing YAML camera calibration:\n%s", e.what());
00222 return false;
00223 }
00224 }
00225
00226 bool readCalibrationYml(const std::string& file_name, std::string& camera_name,
00227 sensor_msgs::CameraInfo& cam_info)
00228 {
00229 std::ifstream fin(file_name.c_str());
00230 if (!fin.good()) {
00231 ROS_INFO("Unable to open camera calibration file [%s]", file_name.c_str());
00232 return false;
00233 }
00234 bool success = readCalibrationYml(fin, camera_name, cam_info);
00235 if (!success)
00236 ROS_ERROR("Failed to parse camera calibration from file [%s]", file_name.c_str());
00237 return success;
00238 }
00239
00240 }