undistort_tof.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_camera_sensors
00013  *
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *
00016  * Author: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00017  * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00018  *
00019  * Date of creation: Dec 2010
00020  * ToDo:
00021  *
00022  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00023  *
00024  * Redistribution and use in source and binary forms, with or without
00025  * modification, are permitted provided that the following conditions are met:
00026  *
00027  *     * Redistributions of source code must retain the above copyright
00028  *       notice, this list of conditions and the following disclaimer.
00029  *     * Redistributions in binary form must reproduce the above copyright
00030  *       notice, this list of conditions and the following disclaimer in the
00031  *       documentation and/or other materials provided with the distribution.
00032  *     * Neither the name of the Fraunhofer Institute for Manufacturing
00033  *       Engineering and Automation (IPA) nor the names of its
00034  *       contributors may be used to endorse or promote products derived from
00035  *       this software without specific prior written permission.
00036  *
00037  * This program is free software: you can redistribute it and/or modify
00038  * it under the terms of the GNU Lesser General Public License LGPL as
00039  * published by the Free Software Foundation, either version 3 of the
00040  * License, or (at your option) any later version.
00041  *
00042  * This program is distributed in the hope that it will be useful,
00043  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00044  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00045  * GNU Lesser General Public License LGPL for more details.
00046  *
00047  * You should have received a copy of the GNU Lesser General Public
00048  * License LGPL along with this program.
00049  * If not, see <http://www.gnu.org/licenses/>.
00050  *
00051  ****************************************************************/
00052 
00053 //##################
00054 //#### includes ####
00055 
00056 // standard includes
00057 //--
00058 
00059 // ROS includes
00060 #include <ros/ros.h>
00061 
00062 #include <message_filters/subscriber.h>
00063 #include <message_filters/sync_policies/approximate_time.h>
00064 #include <message_filters/synchronizer.h>
00065 
00066 // ROS message includes
00067 #include <sensor_msgs/PointCloud2.h>
00068 #include <sensor_msgs/CameraInfo.h>
00069 
00070 // external includes
00071 #include <opencv/cv.h>
00072 #include <opencv/highgui.h>
00073 
00074 using namespace message_filters;
00075 
00076 typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> SyncPolicy;
00077 
00078 
00079 class UndistortTOF
00080 {
00081 private:
00082         ros::NodeHandle node_handle_;
00083         sensor_msgs::PointCloud2 pc2_msg_;
00084 
00085         message_filters::Subscriber<sensor_msgs::PointCloud2> sub_pc2_;
00086         message_filters::Subscriber<sensor_msgs::CameraInfo> sub_camera_info_;
00087         message_filters::Synchronizer<SyncPolicy> sub_sync_;
00088         ros::Publisher pub_pc2_;
00089 
00090 public:
00091         UndistortTOF(const ros::NodeHandle& node_handle)
00092         : node_handle_(node_handle),
00093           sub_sync_(SyncPolicy(3))
00094           {
00095                 sub_sync_.connectInput(sub_pc2_, sub_camera_info_);
00096                 sub_sync_.registerCallback(boost::bind(&UndistortTOF::Undistort, this, _1, _2));
00097                 sub_pc2_.subscribe(node_handle_, "tof/point_cloud2", 1);
00098                 sub_camera_info_.subscribe(node_handle_, "tof/camera_info", 1);
00099                 pub_pc2_ = node_handle_.advertise<sensor_msgs::PointCloud2>("point_cloud_undistorted", 1);
00100           }
00101 
00102         // Return value is in m
00103         unsigned long GetCalibratedZSwissranger(int u, int v, int width, float& zCalibrated)
00104         {
00105                 //zCalibrated = (float) m_Z[v*width + u];
00106 
00107                 //return RET_OK;
00108         }
00109 
00110         // u and v are assumed to be distorted coordinates
00111         unsigned long GetCalibratedXYMatlab(int u, int v, float z, float& x, float& y)
00112         {
00113                 // Use intrinsic camera parameters
00114                 /*double fx, fy, cx, cy;
00115 
00116                 fx = m_intrinsicMatrix.at<double>(0, 0);
00117                 fy = m_intrinsicMatrix.at<double>(1, 1);
00118 
00119                 cx = m_intrinsicMatrix.at<double>(0, 2);
00120                 cy = m_intrinsicMatrix.at<double>(1, 2);
00121 
00122                 // Fundamental equation: u = (fx*x)/z + cx
00123                 if (fx == 0)
00124                 {
00125                         std::cerr << "ERROR - Swissranger::GetCalibratedXYZ:" << std::endl;
00126                         std::cerr << "\t ... fx is 0.\n";
00127                         return RET_FAILED;
00128                 }
00129                 x = (float) (z*(u-cx)/fx) ;
00130 
00131                 // Fundamental equation: v = (fy*y)/z + cy
00132                 if (fy == 0)
00133                 {
00134                         std::cerr << "ERROR - Swissranger::GetCalibratedXYZ:" << std::endl;
00135                         std::cerr << "\t ... fy is 0.\n";
00136                         return RET_FAILED;
00137                 }
00138                 y = (float) (z*(v-cy)/fy);
00139 
00140                 return RET_OK;*/
00141         }
00142 
00143         //void Undistort(const sensor_msgs::PointCloud2ConstPtr& tof_camera_data, const sensor_msgs::CameraInfoConstPtr& camera_info)
00144         void Undistort(const boost::shared_ptr<sensor_msgs::PointCloud2 const>& tof_camera_data, const sensor_msgs::CameraInfoConstPtr& camera_info)
00145         {
00146                 cv::Mat D = cv::Mat(1,4,CV_64FC1);
00147                 D.at<double>(0,0) = camera_info->D[0];
00148                 D.at<double>(0,1) = camera_info->D[1];
00149                 D.at<double>(0,2) = camera_info->D[2];
00150                 D.at<double>(0,3) = camera_info->D[3];
00151                 cv::Mat cam_matrix = cv::Mat::zeros(3,3,CV_64FC1);
00152                 cam_matrix.at<double>(0,0) = camera_info->K[0];
00153                 cam_matrix.at<double>(0,2) = camera_info->K[2];
00154                 cam_matrix.at<double>(1,1) = camera_info->K[4];
00155                 cam_matrix.at<double>(1,2) = camera_info->K[5];
00156                 cam_matrix.at<double>(2,2) = 1;
00157 
00158                 int z_offset = 0, i_offset = 0, c_offset = 0, x_offset = 0, y_offset = 0;;
00159                 for (size_t d = 0; d < tof_camera_data->fields.size(); ++d)
00160                 {
00161                         if(tof_camera_data->fields[d].name == "x")
00162                                 x_offset = tof_camera_data->fields[d].offset;
00163                         if(tof_camera_data->fields[d].name == "y")
00164                                 y_offset = tof_camera_data->fields[d].offset;
00165                         if(tof_camera_data->fields[d].name == "z")
00166                                 z_offset = tof_camera_data->fields[d].offset;
00167                         if(tof_camera_data->fields[d].name == "intensity")
00168                                 i_offset = tof_camera_data->fields[d].offset;
00169                         if(tof_camera_data->fields[d].name == "confidence")
00170                                 c_offset = tof_camera_data->fields[d].offset;
00171                 }
00172                 cv::Mat z_image = cv::Mat(tof_camera_data->height, tof_camera_data->width,CV_32FC1);
00173                 cv::Mat intensity_image = cv::Mat(tof_camera_data->height, tof_camera_data->width,CV_32FC1);
00174                 float* f_ptr = 0;
00175                 float* i_ptr = 0;
00176                 int pc_msg_idx=0;
00177                 for (int row = 0; row < z_image.rows; row++)
00178                 {
00179                         f_ptr = z_image.ptr<float>(row);
00180                         i_ptr = intensity_image.ptr<float>(row);
00181                         for (int col = 0; col < z_image.cols; col++, pc_msg_idx++)
00182                         {
00183                                 memcpy(&f_ptr[col], &tof_camera_data->data[pc_msg_idx * tof_camera_data->point_step + z_offset], sizeof(float));
00184                                 memcpy(&i_ptr[col], &tof_camera_data->data[pc_msg_idx * tof_camera_data->point_step + i_offset], sizeof(float));
00185                         }
00186                 }
00187 
00188                 cv::imshow("distorted", z_image);
00189                 // Undistort
00190                 cv::Mat z_image_undistorted;
00191                 cv::undistort(z_image, z_image_undistorted, cam_matrix, D);
00192                 cv::Mat intensity_image_undistorted;
00193                 cv::undistort(intensity_image, intensity_image_undistorted, cam_matrix, D);
00194 
00195                 cv::imshow("undistorted", z_image_undistorted);
00196                 cv::waitKey(20);
00197 
00198                 sensor_msgs::PointCloud2 pc_pub = *(tof_camera_data.get());
00199                 // Calculate X and Y based on instrinsic rotation and translation
00200                 double fx, fy, cx, cy;
00201 
00202                 fx = cam_matrix.at<double>(0,0);
00203                 fy = cam_matrix.at<double>(1, 1);
00204 
00205                 cx = cam_matrix.at<double>(0, 2);
00206                 cy = cam_matrix.at<double>(1, 2);
00207 
00208 
00209                 for (size_t d = 0; d < pc_pub.fields.size(); ++d)
00210                 {
00211                 }
00212                 for(unsigned int row=0; row<pc_pub.height; row++)
00213                 {
00214                         float* z =  z_image_undistorted.ptr<float>(row);
00215                         //f_ptr = (float*)(cartesianImageData + row*widthStepCartesianImage);
00216 
00217                         for (unsigned int col=0; col<pc_pub.width; col++)
00218                         {
00219                                 memcpy(&pc_pub.data[row * col * pc_pub.point_step + z_offset], &z[col], sizeof(float));
00220                                 float x = (float) (z[col]*(col-cx)/fx);
00221                                 float y =  (float) (z[col]*(row-cy)/fy);
00222                                 memcpy(&pc_pub.data[row * col * pc_pub.point_step + x_offset], &x, sizeof(float));
00223                                 memcpy(&pc_pub.data[row * col * pc_pub.point_step + y_offset], &y, sizeof(float));
00224                         }
00225                 }
00226                 pub_pc2_.publish(pc_pub);
00227         }
00228 
00229 };
00230 
00231 
00232 //#######################
00233 //#### main programm ####
00234 int main(int argc, char** argv)
00235 {
00237         ros::init(argc, argv, "undistort_tof");
00238 
00240         ros::NodeHandle nh;
00241 
00243         UndistortTOF undistort_tof(nh);
00244 
00246         //if (!camera_node.init()) return 0;
00247 
00248         ros::spin();
00249 
00250         return 0;
00251 }


cob_camera_sensors
Author(s): Jan Fischer
autogenerated on Thu Aug 27 2015 12:45:58