calibration.cc

Go to the documentation of this file.

#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <yaml-cpp/yaml.h>

#include <ros/ros.h>
#include <velodyne_pointcloud/calibration.h>

namespace velodyne_pointcloud {

  const std::string NUM_LASERS = "num_lasers";
  const std::string LASERS = "lasers";
  const std::string LASER_ID = "laser_id";
  const std::string ROT_CORRECTION = "rot_correction";
  const std::string VERT_CORRECTION = "vert_correction";
  const std::string DIST_CORRECTION = "dist_correction";
  const std::string TWO_PT_CORRECTION_AVAILABLE = "two_pt_correction_available";
  const std::string DIST_CORRECTION_X = "dist_correction_x";
  const std::string DIST_CORRECTION_Y = "dist_correction_y";
  const std::string VERT_OFFSET_CORRECTION = "vert_offset_correction";
  const std::string HORIZ_OFFSET_CORRECTION = "vert_offset_correction";
  const std::string MAX_INTENSITY = "max_intensity";
  const std::string MIN_INTENSITY = "min_intensity";
  const std::string FOCAL_DISTANCE = "focal_distance";
  const std::string FOCAL_SLOPE = "focal_slope";

  void operator >> (const YAML::Node& node, std::pair<int, LaserCorrection>& correction) {
    node[LASER_ID] >> correction.first;
    node[ROT_CORRECTION] >> correction.second.rot_correction;
    node[VERT_CORRECTION] >> correction.second.vert_correction;
    node[DIST_CORRECTION] >> correction.second.dist_correction;
    if (const YAML::Node *pName = node.FindValue(TWO_PT_CORRECTION_AVAILABLE))
      *pName >> correction.second.two_pt_correction_available;
    else
      correction.second.two_pt_correction_available = false;
    node[DIST_CORRECTION_X] >> correction.second.dist_correction_x;
    node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y;
    node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction;
    node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction;
    if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY))
      *pName >> correction.second.max_intensity;
    else
      correction.second.max_intensity = 255;
    if (const YAML::Node *pName = node.FindValue(MIN_INTENSITY))
      *pName >> correction.second.min_intensity;
    else
      correction.second.min_intensity = 0;
    node[FOCAL_DISTANCE] >> correction.second.focal_distance;
    node[FOCAL_SLOPE] >> correction.second.focal_slope;

    // Calculate cached values
    correction.second.cos_rot_correction = cosf(correction.second.rot_correction);
    correction.second.sin_rot_correction = sinf(correction.second.rot_correction);
    correction.second.cos_vert_correction = cosf(correction.second.vert_correction);
    correction.second.sin_vert_correction = sinf(correction.second.vert_correction);

    correction.second.laser_ring = 0;   // clear initially (set later)
  }

  void operator >> (const YAML::Node& node, Calibration& calibration) {
    int num_lasers;
    node[NUM_LASERS] >> num_lasers;
    const YAML::Node& lasers = node[LASERS];
    calibration.laser_corrections.clear();
    for (int i = 0; i < num_lasers; i++) {
      std::pair<int, LaserCorrection> correction;
      lasers[i] >> correction;
      calibration.laser_corrections.insert(correction);
    }

    // For each laser ring, find the next-smallest vertical angle.
    //
    // This implementation is simple, but not efficient.  That is OK,
    // since it only runs while starting up.
    double next_angle = -std::numeric_limits<double>::infinity();
    for (int ring = 0; ring < num_lasers; ++ring) {

      // find minimum remaining vertical offset correction
      double min_seen = std::numeric_limits<double>::infinity();
      int next_index = num_lasers;
      for (int j = 0; j < num_lasers; ++j) {

        double angle = calibration.laser_corrections[j].vert_correction;
        if (next_angle < angle && angle < min_seen) {
          min_seen = angle;
          next_index = j;
        }
      }

      if (next_index < num_lasers) {    // anything found in this ring?

        // store this ring number with its corresponding laser number
        calibration.laser_corrections[next_index].laser_ring = ring;
        next_angle = min_seen;
        ROS_INFO_STREAM("laser_ring[" << next_index << "] = " << ring
                         << ", angle = " << next_angle);
      }
    }
  }

  YAML::Emitter& operator << (YAML::Emitter& out, const std::pair<int, LaserCorrection> correction) {
    out << YAML::BeginMap;
    out << YAML::Key << LASER_ID << YAML::Value << correction.first;
    out << YAML::Key << ROT_CORRECTION << YAML::Value << correction.second.rot_correction;
    out << YAML::Key << VERT_CORRECTION << YAML::Value << correction.second.vert_correction;
    out << YAML::Key << DIST_CORRECTION << YAML::Value << correction.second.dist_correction;
    out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE << YAML::Value << correction.second.two_pt_correction_available;
    out << YAML::Key << DIST_CORRECTION_X << YAML::Value << correction.second.dist_correction_x;
    out << YAML::Key << DIST_CORRECTION_Y << YAML::Value << correction.second.dist_correction_y;
    out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value << correction.second.vert_offset_correction;
    out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value << correction.second.horiz_offset_correction;
    out << YAML::Key << MAX_INTENSITY << YAML::Value << correction.second.max_intensity;
    out << YAML::Key << MIN_INTENSITY << YAML::Value << correction.second.min_intensity;
    out << YAML::Key << FOCAL_DISTANCE << YAML::Value << correction.second.focal_distance;
    out << YAML::Key << FOCAL_SLOPE << YAML::Value << correction.second.focal_slope;
    out << YAML::EndMap;
    return out;
  }

  YAML::Emitter& operator << (YAML::Emitter& out, const Calibration& calibration) {
    out << YAML::BeginMap;
    out << YAML::Key << NUM_LASERS << YAML::Value << calibration.laser_corrections.size();
    out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
    for (std::map<int, LaserCorrection>::const_iterator it = calibration.laser_corrections.begin();
         it != calibration.laser_corrections.end(); it++) {
      out << *it; 
    }
    out << YAML::EndSeq;
    out << YAML::EndMap;
    return out;
  }

  void Calibration::read(const std::string& calibration_file) {
    std::ifstream fin(calibration_file.c_str());
    if (!fin.is_open()) {
      initialized = false;
      return;
    }
    initialized = true;
    try {
      YAML::Parser parser(fin);
      YAML::Node doc;
      parser.GetNextDocument(doc);
      doc >> *this;
    } catch (YAML::Exception &e) {
      std::cerr << "YAML Exception: " << e.what() << std::endl;
      initialized = false;
    }
    fin.close();
  }

  void Calibration::write(const std::string& calibration_file) {
    std::ofstream fout(calibration_file.c_str());
    YAML::Emitter out;
    out << *this;
    fout << out.c_str();
    fout.close();
  }
  
} /* velodyne_pointcloud */