calibration.cpp

Go to the documentation of this file.

#include "sr_utilities/calibration.hpp"
#include "sr_utilities/sr_math_utils.hpp"

#include <boost/assert.hpp>
#include <boost/foreach.hpp>

#include <algorithm>
#include <vector>
#include <iostream>

namespace shadow_robot
{
  JointCalibration::JointCalibration(std::vector<joint_calibration::Point> calibration_table)
  {
    this->calibration_table_ = calibration_table;
    // calibration size - 1 because we use this number to access the last value in the vector
    if (calibration_table.size())
      calibration_table_size_ = calibration_table.size() - 1;
    else
      calibration_table_size_ = 0;

    // fails if we have only one calibration point as it's not possible
    // to interpolate from one point
    BOOST_ASSERT(calibration_table_size_ > 0);

    /*
     * make sure that the given calibration table is ordered by
     * growing values of the raw_value
     */
    std::sort(this->calibration_table_.begin(), this->calibration_table_.end(),
              joint_calibration::sort_growing_raw_operator);
  }

  double JointCalibration::compute(double raw_reading)
  {
    joint_calibration::Point low_point, high_point;

    // That takes care of computing a reading that's before
    // the calibration table as well as a reading that's in the
    // first calibration table case.
    low_point = calibration_table_[0];
    high_point = calibration_table_[1];

    bool found = false;

    // if we have more than 2 points in our calibration table
    // or if the raw value isn't before the calibration table
    if (raw_reading > calibration_table_[0].raw_value)
    {
      if (calibration_table_size_ > 1)
      {
        for (unsigned int index_cal = 1; index_cal < calibration_table_size_; ++index_cal)
        {
          if ((raw_reading >= calibration_table_[index_cal - 1].raw_value) &&
              (raw_reading < calibration_table_[index_cal].raw_value))
          {
            low_point = calibration_table_[index_cal - 1];
            high_point = calibration_table_[index_cal];

            found = true;
            break;
          }
        }  // end for

        // the point is outside of the table
        if (!found)
        {
          low_point = calibration_table_[calibration_table_size_ - 1];
          high_point = calibration_table_[calibration_table_size_];
        }
      }  // end if 2 values only in the table
    }  // end if raw_reading before table

    return sr_math_utils::linear_interpolate_(raw_reading, low_point.raw_value,
                                              low_point.calibrated_value,
                                              high_point.raw_value,
                                              high_point.calibrated_value);
  }
}  // namespace shadow_robot

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/