calibration.cc
Go to the documentation of this file.
00001 
00013 #include <iostream>
00014 #include <fstream>
00015 #include <string>
00016 #include <cmath>
00017 #include <limits>
00018 #include <yaml-cpp/yaml.h>
00019 
00020 #include <ros/ros.h>
00021 #include <velodyne_pointcloud/calibration.h>
00022 
00023 namespace velodyne_pointcloud {
00024 
00025   const std::string NUM_LASERS = "num_lasers";
00026   const std::string LASERS = "lasers";
00027   const std::string LASER_ID = "laser_id";
00028   const std::string ROT_CORRECTION = "rot_correction";
00029   const std::string VERT_CORRECTION = "vert_correction";
00030   const std::string DIST_CORRECTION = "dist_correction";
00031   const std::string TWO_PT_CORRECTION_AVAILABLE = "two_pt_correction_available";
00032   const std::string DIST_CORRECTION_X = "dist_correction_x";
00033   const std::string DIST_CORRECTION_Y = "dist_correction_y";
00034   const std::string VERT_OFFSET_CORRECTION = "vert_offset_correction";
00035   const std::string HORIZ_OFFSET_CORRECTION = "vert_offset_correction";
00036   const std::string MAX_INTENSITY = "max_intensity";
00037   const std::string MIN_INTENSITY = "min_intensity";
00038   const std::string FOCAL_DISTANCE = "focal_distance";
00039   const std::string FOCAL_SLOPE = "focal_slope";
00040 
00041   void operator >> (const YAML::Node& node, std::pair<int, LaserCorrection>& correction) {
00042     node[LASER_ID] >> correction.first;
00043     node[ROT_CORRECTION] >> correction.second.rot_correction;
00044     node[VERT_CORRECTION] >> correction.second.vert_correction;
00045     node[DIST_CORRECTION] >> correction.second.dist_correction;
00046     if (const YAML::Node *pName = node.FindValue(TWO_PT_CORRECTION_AVAILABLE))
00047       *pName >> correction.second.two_pt_correction_available;
00048     else
00049       correction.second.two_pt_correction_available = false;
00050     node[DIST_CORRECTION_X] >> correction.second.dist_correction_x;
00051     node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y;
00052     node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction;
00053     node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction;
00054     if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY))
00055       *pName >> correction.second.max_intensity;
00056     else
00057       correction.second.max_intensity = 255;
00058     if (const YAML::Node *pName = node.FindValue(MIN_INTENSITY))
00059       *pName >> correction.second.min_intensity;
00060     else
00061       correction.second.min_intensity = 0;
00062     node[FOCAL_DISTANCE] >> correction.second.focal_distance;
00063     node[FOCAL_SLOPE] >> correction.second.focal_slope;
00064 
00065     // Calculate cached values
00066     correction.second.cos_rot_correction = cosf(correction.second.rot_correction);
00067     correction.second.sin_rot_correction = sinf(correction.second.rot_correction);
00068     correction.second.cos_vert_correction = cosf(correction.second.vert_correction);
00069     correction.second.sin_vert_correction = sinf(correction.second.vert_correction);
00070 
00071     correction.second.laser_ring = 0;   // clear initially (set later)
00072   }
00073 
00074   void operator >> (const YAML::Node& node, Calibration& calibration) {
00075     int num_lasers;
00076     node[NUM_LASERS] >> num_lasers;
00077     const YAML::Node& lasers = node[LASERS];
00078     calibration.laser_corrections.clear();
00079     for (int i = 0; i < num_lasers; i++) {
00080       std::pair<int, LaserCorrection> correction;
00081       lasers[i] >> correction;
00082       calibration.laser_corrections.insert(correction);
00083     }
00084 
00085     // For each laser ring, find the next-smallest vertical angle.
00086     //
00087     // This implementation is simple, but not efficient.  That is OK,
00088     // since it only runs while starting up.
00089     double next_angle = -std::numeric_limits<double>::infinity();
00090     for (int ring = 0; ring < num_lasers; ++ring) {
00091 
00092       // find minimum remaining vertical offset correction
00093       double min_seen = std::numeric_limits<double>::infinity();
00094       int next_index = num_lasers;
00095       for (int j = 0; j < num_lasers; ++j) {
00096 
00097         double angle = calibration.laser_corrections[j].vert_correction;
00098         if (next_angle < angle && angle < min_seen) {
00099           min_seen = angle;
00100           next_index = j;
00101         }
00102       }
00103 
00104       if (next_index < num_lasers) {    // anything found in this ring?
00105 
00106         // store this ring number with its corresponding laser number
00107         calibration.laser_corrections[next_index].laser_ring = ring;
00108         next_angle = min_seen;
00109         ROS_INFO_STREAM("laser_ring[" << next_index << "] = " << ring
00110                          << ", angle = " << next_angle);
00111       }
00112     }
00113   }
00114 
00115   YAML::Emitter& operator << (YAML::Emitter& out, const std::pair<int, LaserCorrection> correction) {
00116     out << YAML::BeginMap;
00117     out << YAML::Key << LASER_ID << YAML::Value << correction.first;
00118     out << YAML::Key << ROT_CORRECTION << YAML::Value << correction.second.rot_correction;
00119     out << YAML::Key << VERT_CORRECTION << YAML::Value << correction.second.vert_correction;
00120     out << YAML::Key << DIST_CORRECTION << YAML::Value << correction.second.dist_correction;
00121     out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE << YAML::Value << correction.second.two_pt_correction_available;
00122     out << YAML::Key << DIST_CORRECTION_X << YAML::Value << correction.second.dist_correction_x;
00123     out << YAML::Key << DIST_CORRECTION_Y << YAML::Value << correction.second.dist_correction_y;
00124     out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value << correction.second.vert_offset_correction;
00125     out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value << correction.second.horiz_offset_correction;
00126     out << YAML::Key << MAX_INTENSITY << YAML::Value << correction.second.max_intensity;
00127     out << YAML::Key << MIN_INTENSITY << YAML::Value << correction.second.min_intensity;
00128     out << YAML::Key << FOCAL_DISTANCE << YAML::Value << correction.second.focal_distance;
00129     out << YAML::Key << FOCAL_SLOPE << YAML::Value << correction.second.focal_slope;
00130     out << YAML::EndMap;
00131     return out;
00132   }
00133 
00134   YAML::Emitter& operator << (YAML::Emitter& out, const Calibration& calibration) {
00135     out << YAML::BeginMap;
00136     out << YAML::Key << NUM_LASERS << YAML::Value << calibration.laser_corrections.size();
00137     out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
00138     for (std::map<int, LaserCorrection>::const_iterator it = calibration.laser_corrections.begin();
00139          it != calibration.laser_corrections.end(); it++) {
00140       out << *it; 
00141     }
00142     out << YAML::EndSeq;
00143     out << YAML::EndMap;
00144     return out;
00145   }
00146 
00147   void Calibration::read(const std::string& calibration_file) {
00148     std::ifstream fin(calibration_file.c_str());
00149     if (!fin.is_open()) {
00150       initialized = false;
00151       return;
00152     }
00153     initialized = true;
00154     try {
00155       YAML::Parser parser(fin);
00156       YAML::Node doc;
00157       parser.GetNextDocument(doc);
00158       doc >> *this;
00159     } catch (YAML::Exception &e) {
00160       std::cerr << "YAML Exception: " << e.what() << std::endl;
00161       initialized = false;
00162     }
00163     fin.close();
00164   }
00165 
00166   void Calibration::write(const std::string& calibration_file) {
00167     std::ofstream fout(calibration_file.c_str());
00168     YAML::Emitter out;
00169     out << *this;
00170     fout << out.c_str();
00171     fout.close();
00172   }
00173   
00174 } /* velodyne_pointcloud */


velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera
autogenerated on Mon Oct 6 2014 08:36:37