apply_tof_calibration_alg.cpp
Go to the documentation of this file.
00001 #include "apply_tof_calibration_alg.h"
00002 
00003 ApplyTofCalibrationAlgorithm::ApplyTofCalibrationAlgorithm(void)
00004 {
00005   /*
00006    * Load camera parameters
00007    */
00008   //tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_spadmeter_with_depth/extrinsic_swr_calib_depth_20110205_raw_01000_VARIII_2.par");
00009   //tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_spadmeter_with_depth/extrinsic_20120127.par");
00010   //tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_spadmeter_with_depth/extrinsic_20121016.par");
00011   tofCameraModel_ = new TOFCameraModel("/home/sfoix/programming/data/tof_camboard/hand_eye_camboard_2013_02_20/frames/extrinsic_20120127.par");
00012 }
00013 
00014 ApplyTofCalibrationAlgorithm::~ApplyTofCalibrationAlgorithm(void)
00015 {
00016   delete tofCameraModel_;
00017 }
00018 
00019 void ApplyTofCalibrationAlgorithm::config_update(Config& new_cfg, uint32_t level)
00020 {
00021   this->lock();
00022 
00023   // save the current configuration
00024   this->config_=new_cfg;
00025   
00026   this->unlock();
00027 }
00028 
00029 // ApplyTofCalibrationAlgorithm Public API
00030 bool ApplyTofCalibrationAlgorithm::apply_tof_calibration_to_pcl2(const sensor_msgs::PointCloud2::ConstPtr& msg){
00031 
00032   // DATA CONVERSION 
00033   // convert *msg to pcl object
00034   //pcl::fromROSMsg(*msg, calibrated_pcl_);
00035   calibrated_pcl_ = *msg;
00036 
00037   // create depth and amplitude arrays
00038   // convert depth from millimeters[mm] to meters[m]
00039   double* lastDepth = new double[calibrated_pcl_.height*calibrated_pcl_.width];
00040   double* lastAmp = new double[calibrated_pcl_.height*calibrated_pcl_.width];
00041   unsigned int ii = 0;
00042   //pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter = calibrated_pcl_.begin();
00043   for (size_t rr =0; rr < calibrated_pcl_.height; ++rr){
00044       //for (int cc = 0; cc < calibrated_pcl_.width; ++cc, ++pt_iter, ++ii){
00045       for (size_t cc = 0; cc < calibrated_pcl_.width; ++cc, ++ii){
00046           int index = rr * calibrated_pcl_.width + cc;
00047           float *pstep = (float*)&calibrated_pcl_.data[(index) * calibrated_pcl_.point_step];
00048           lastDepth[ii] = pstep[2]*1000;
00049           lastAmp[ii] = pstep[3];
00050       }
00051   }
00052   
00053   tofCameraModel_->correctDepthValues(lastDepth, lastAmp);
00054   // Not necessary since we undistort the pointcloud at a more low level
00055   //tofCameraModel_->undistort(lastDepth,lastAmp);
00056 
00057   //pt_iter = calibrated_pcl_.begin();
00058   ii = 0;
00059   for (int rr =0; rr < (int)calibrated_pcl_.height; ++rr){
00060       //for (int cc = 0; cc < (int)calibrated_pcl_.width; ++cc, ++pt_iter, ++ii){
00061       for (int cc = 0; cc < (int)calibrated_pcl_.width; ++cc, ++ii){
00062           int index = rr * calibrated_pcl_.width + cc;
00063           float *pstep = (float*)&calibrated_pcl_.data[(index) * calibrated_pcl_.point_step];
00064           pstep[2] = lastDepth[ii]/1000;
00065           pstep[3] = lastAmp[ii];
00066           //pcl::PointXYZRGB &pt = *pt_iter;
00067           //pt.z = lastDepth[ii]/1000;
00068           //pt.rgb = lastAmp[ii];
00069       }
00070   }
00071 
00072   delete [] lastDepth;
00073   delete [] lastAmp;
00074   return true;
00075 }
00076 
00077 //pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr ApplyTofCalibrationAlgorithm::get_calibrated_pcl(void)
00078 sensor_msgs::PointCloud2* ApplyTofCalibrationAlgorithm::get_calibrated_pcl(void)
00079 {
00080   return &calibrated_pcl_;
00081 }
00082 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


iri_apply_tof_calibration
Author(s): Sergi Foix
autogenerated on Wed Oct 9 2013 12:40:17