37 #include <boost/filesystem.hpp> 38 #include <yaml-cpp/yaml.h> 49 static const char CAM_YML_NAME[] =
"camera_name";
50 static const char WIDTH_YML_NAME[] =
"image_width";
51 static const char HEIGHT_YML_NAME[] =
"image_height";
52 static const char K_YML_NAME[] =
"camera_matrix";
53 static const char D_YML_NAME[] =
"distortion_coefficients";
54 static const char R_YML_NAME[] =
"rectification_matrix";
55 static const char P_YML_NAME[] =
"projection_matrix";
56 static const char DMODEL_YML_NAME[] =
"distortion_model";
64 SimpleMatrix(
int rows,
int cols,
double* data)
65 : rows(rows), cols(cols), data(data)
69 YAML::Emitter& operator << (YAML::Emitter& out,
const SimpleMatrix& m)
71 out << YAML::BeginMap;
72 out << YAML::Key <<
"rows" << YAML::Value << m.rows;
73 out << YAML::Key <<
"cols" << YAML::Value << m.cols;
75 out << YAML::Key <<
"data" << YAML::Value;
77 out << YAML::BeginSeq;
78 for (
int i = 0; i < m.rows*m.cols; ++i)
85 #ifdef HAVE_NEW_YAMLCPP 87 void operator >> (
const YAML::Node& node, T& i)
93 void operator >> (
const YAML::Node& node, SimpleMatrix& m)
97 assert(rows == m.rows);
99 assert(cols == m.cols);
100 const YAML::Node& data = node[
"data"];
101 for (
int i = 0; i < rows*cols; ++i)
102 data[i] >> m.data[i];
108 const sensor_msgs::CameraInfo& cam_info)
110 YAML::Emitter emitter;
111 emitter << YAML::BeginMap;
118 emitter << YAML::Key <<
"calibration_time";
119 emitter << YAML::Value << asctime(localtime(&raw_time));
123 emitter << YAML::Key << WIDTH_YML_NAME << YAML::Value << (int)cam_info.width;
124 emitter << YAML::Key << HEIGHT_YML_NAME << YAML::Value << (
int)cam_info.height;
127 emitter << YAML::Key << CAM_YML_NAME << YAML::Value << camera_name;
128 emitter << YAML::Key << K_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.K[0]));
129 emitter << YAML::Key << DMODEL_YML_NAME << YAML::Value << cam_info.distortion_model;
130 emitter << YAML::Key << D_YML_NAME << YAML::Value << SimpleMatrix(1, cam_info.D.size(),
131 const_cast<double*
>(&cam_info.D[0]));
132 emitter << YAML::Key << R_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast<double*>(&cam_info.R[0]));
133 emitter << YAML::Key << P_YML_NAME << YAML::Value << SimpleMatrix(3, 4, const_cast<double*>(&cam_info.P[0]));
135 emitter << YAML::EndMap;
137 out << emitter.c_str();
142 const sensor_msgs::CameraInfo& cam_info)
144 boost::filesystem::path dir(boost::filesystem::path(file_name).parent_path());
145 if (!dir.empty() && !boost::filesystem::exists(dir) &&
146 !boost::filesystem::create_directories(dir)){
147 ROS_ERROR(
"Unable to create directory for camera calibration file [%s]", dir.c_str());
149 std::ofstream out(file_name.c_str());
152 ROS_ERROR(
"Unable to open camera calibration file [%s] for writing", file_name.c_str());
158 bool readCalibrationYml(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info)
161 #ifdef HAVE_NEW_YAMLCPP 162 YAML::Node doc = YAML::Load(in);
164 if (doc[CAM_YML_NAME])
165 doc[CAM_YML_NAME] >> camera_name;
167 camera_name =
"unknown";
171 ROS_ERROR(
"Unable to create YAML parser for camera calibration");
175 parser.GetNextDocument(doc);
177 if (
const YAML::Node* name_node = doc.FindValue(CAM_YML_NAME))
178 *name_node >> camera_name;
180 camera_name =
"unknown";
183 doc[WIDTH_YML_NAME] >> cam_info.width;
184 doc[HEIGHT_YML_NAME] >> cam_info.height;
187 SimpleMatrix K_(3, 3, &cam_info.K[0]);
188 doc[K_YML_NAME] >> K_;
189 SimpleMatrix R_(3, 3, &cam_info.R[0]);
190 doc[R_YML_NAME] >> R_;
191 SimpleMatrix P_(3, 4, &cam_info.P[0]);
192 doc[P_YML_NAME] >> P_;
195 #ifdef HAVE_NEW_YAMLCPP 196 if (doc[DMODEL_YML_NAME]) {
197 doc[DMODEL_YML_NAME] >> cam_info.distortion_model;
200 if (
const YAML::Node* model_node = doc.FindValue(DMODEL_YML_NAME)) {
201 *model_node >> cam_info.distortion_model;
207 ROS_WARN(
"Camera calibration file did not specify distortion model, assuming plumb bob");
209 const YAML::Node& D_node = doc[D_YML_NAME];
211 D_node[
"rows"] >> D_rows;
212 D_node[
"cols"] >> D_cols;
213 const YAML::Node& D_data = D_node[
"data"];
214 cam_info.D.resize(D_rows*D_cols);
215 for (
int i = 0; i < D_rows*D_cols; ++i)
216 D_data[i] >> cam_info.D[i];
220 catch (YAML::Exception& e) {
221 ROS_ERROR(
"Exception parsing YAML camera calibration:\n%s", e.what());
227 sensor_msgs::CameraInfo& cam_info)
229 std::ifstream fin(file_name.c_str());
231 ROS_INFO(
"Unable to open camera calibration file [%s]", file_name.c_str());
236 ROS_ERROR(
"Failed to parse camera calibration from file [%s]", file_name.c_str());
bool readCalibrationYml(std::istream &in, std::string &camera_name, sensor_msgs::CameraInfo &cam_info)
Read calibration parameters from a YAML file.
const std::string PLUMB_BOB
bool writeCalibrationYml(std::ostream &out, const std::string &camera_name, const sensor_msgs::CameraInfo &cam_info)
Write calibration parameters to a file in YAML format.