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