apply_tof_calibration_alg.cpp

Go to the documentation of this file.

#include "apply_tof_calibration_alg.h"

ApplyTofCalibrationAlgorithm::ApplyTofCalibrationAlgorithm(void)
{
  /*
   * Load camera parameters
   */
  //tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_spadmeter_with_depth/extrinsic_swr_calib_depth_20110205_raw_01000_VARIII_2.par");
  //tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_spadmeter_with_depth/extrinsic_20120127.par");
  //tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_spadmeter_with_depth/extrinsic_20121016.par");
  tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_camboard_2013_02_20/frames/extrinsic_20120127.par");
}

ApplyTofCalibrationAlgorithm::~ApplyTofCalibrationAlgorithm(void)
{
  delete tofCameraModel_;
}

void ApplyTofCalibrationAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// ApplyTofCalibrationAlgorithm Public API
bool ApplyTofCalibrationAlgorithm::apply_tof_calibration_to_pcl2(const sensor_msgs::PointCloud2::ConstPtr& msg){

  // DATA CONVERSION 
  // convert *msg to pcl object
  //pcl::fromROSMsg(*msg, calibrated_pcl_);
  calibrated_pcl_ = *msg;

  // create depth and amplitude arrays
  // convert depth from millimeters[mm] to meters[m]
  double* lastDepth = new double[calibrated_pcl_.height*calibrated_pcl_.width];
  double* lastAmp = new double[calibrated_pcl_.height*calibrated_pcl_.width];
  unsigned int ii = 0;
  //pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter = calibrated_pcl_.begin();
  for (size_t rr =0; rr < calibrated_pcl_.height; ++rr){
      //for (int cc = 0; cc < calibrated_pcl_.width; ++cc, ++pt_iter, ++ii){
      for (size_t cc = 0; cc < calibrated_pcl_.width; ++cc, ++ii){
          int index = rr * calibrated_pcl_.width + cc;
          float *pstep = (float*)&calibrated_pcl_.data[(index) * calibrated_pcl_.point_step];
          lastDepth[ii] = pstep[2]*1000;
          lastAmp[ii] = pstep[3];
      }
  }
  
  tofCameraModel_->correctDepthValues(lastDepth, lastAmp);
  // Not necessary since we undistort the pointcloud at a more low level
  //tofCameraModel_->undistort(lastDepth,lastAmp);

  //pt_iter = calibrated_pcl_.begin();
  ii = 0;
  for (int rr =0; rr < (int)calibrated_pcl_.height; ++rr){
      //for (int cc = 0; cc < (int)calibrated_pcl_.width; ++cc, ++pt_iter, ++ii){
      for (int cc = 0; cc < (int)calibrated_pcl_.width; ++cc, ++ii){
          int index = rr * calibrated_pcl_.width + cc;
          float *pstep = (float*)&calibrated_pcl_.data[(index) * calibrated_pcl_.point_step];
          pstep[2] = lastDepth[ii]/1000;
          pstep[3] = lastAmp[ii];
          //pcl::PointXYZRGB &pt = *pt_iter;
          //pt.z = lastDepth[ii]/1000;
          //pt.rgb = lastAmp[ii];
      }
  }

  delete [] lastDepth;
  delete [] lastAmp;
  return true;
}

//pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr ApplyTofCalibrationAlgorithm::get_calibrated_pcl(void)
sensor_msgs::PointCloud2* ApplyTofCalibrationAlgorithm::get_calibrated_pcl(void)
{
  return &calibrated_pcl_;
}