$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by Robo@FIT group. 00009 * 00010 * Author: Zdenek Materna (imaterna@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 1/28/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #include <ros/ros.h> 00029 00030 #include <sensor_msgs/PointCloud2.h> 00031 #include <iostream> 00032 #include <fstream> 00033 #include <cmath> 00034 #include <math.h> 00035 #include <pcl_ros/point_cloud.h> 00036 #include <pcl/point_types.h> 00037 #include <pcl/ros/conversions.h> 00038 00039 00040 00041 using namespace std; 00042 using namespace sensor_msgs; 00043 00044 ros::Publisher m_pub; 00045 bool add_noise = true; 00046 double nac = 1.0; // noise amount coef 00047 double max_val = 5000.0; // max depth value in mm 00048 double min_val = 500.0; 00049 00050 00051 00052 /******************************************************************************/ 00053 /* randn() 00054 * 00055 * Normally (Gaussian) distributed random numbers, using the Box-Muller 00056 * transformation. This transformation takes two uniformly distributed deviates 00057 * within the unit circle, and transforms them into two independently 00058 * distributed normal deviates. Utilizes the internal rand() function; this can 00059 * easily be changed to use a better and faster RNG. 00060 * 00061 * The parameters passed to the function are the mean and standard deviation of 00062 * the desired distribution. The default values used, when no arguments are 00063 * passed, are 0 and 1 - the standard normal distribution. 00064 * 00065 * 00066 * Two functions are provided: 00067 * 00068 * The first uses the so-called polar version of the B-M transformation, using 00069 * multiple calls to a uniform RNG to ensure the initial deviates are within the 00070 * unit circle. This avoids making any costly trigonometric function calls. 00071 * 00072 * The second makes only a single set of calls to the RNG, and calculates a 00073 * position within the unit circle with two trigonometric function calls. 00074 * 00075 * The polar version is generally superior in terms of speed; however, on some 00076 * systems, the optimization of the math libraries may result in better 00077 * performance of the second. Try it out on the target system to see which 00078 * works best for you. On my test machine (Athlon 3800+), the non-trig version 00079 * runs at about 3x10^6 calls/s; while the trig version runs at about 00080 * 1.8x10^6 calls/s (-O2 optimization). 00081 * 00082 * 00083 * Example calls: 00084 * randn_notrig(); //returns normal deviate with mean=0.0, std. deviation=1.0 00085 * randn_notrig(5.2,3.0); //returns deviate with mean=5.2, std. deviation=3.0 00086 * 00087 * 00088 * Dependencies - requires <cmath> for the sqrt(), sin(), and cos() calls, and a 00089 * #defined value for PI. 00090 */ 00091 00092 /******************************************************************************/ 00093 // "Polar" version without trigonometric calls 00094 double randn_notrig(double mu=0.0, double sigma=1.0) { 00095 static bool deviateAvailable=false; // flag 00096 static float storedDeviate; // deviate from previous calculation 00097 double polar, rsquared, var1, var2; 00098 00099 // If no deviate has been stored, the polar Box-Muller transformation is 00100 // performed, producing two independent normally-distributed random 00101 // deviates. One is stored for the next round, and one is returned. 00102 if (!deviateAvailable) { 00103 00104 // choose pairs of uniformly distributed deviates, discarding those 00105 // that don't fall within the unit circle 00106 do { 00107 var1=2.0*( double(rand())/double(RAND_MAX) ) - 1.0; 00108 var2=2.0*( double(rand())/double(RAND_MAX) ) - 1.0; 00109 rsquared=var1*var1+var2*var2; 00110 } while ( rsquared>=1.0 || rsquared == 0.0); 00111 00112 // calculate polar tranformation for each deviate 00113 polar=sqrt(-2.0*log(rsquared)/rsquared); 00114 00115 // store first deviate and set flag 00116 storedDeviate=var1*polar; 00117 deviateAvailable=true; 00118 00119 // return second deviate 00120 return var2*polar*sigma + mu; 00121 } 00122 00123 // If a deviate is available from a previous call to this function, it is 00124 // returned, and the flag is set to false. 00125 else { 00126 deviateAvailable=false; 00127 return storedDeviate*sigma + mu; 00128 } 00129 } 00130 00131 00132 /******************************************************************************/ 00133 // Standard version with trigonometric calls 00134 #define PI 3.14159265358979323846 00135 00136 double randn_trig(double mu=0.0, double sigma=1.0) { 00137 static bool deviateAvailable=false; // flag 00138 static float storedDeviate; // deviate from previous calculation 00139 double dist, angle; 00140 00141 // If no deviate has been stored, the standard Box-Muller transformation is 00142 // performed, producing two independent normally-distributed random 00143 // deviates. One is stored for the next round, and one is returned. 00144 if (!deviateAvailable) { 00145 00146 // choose a pair of uniformly distributed deviates, one for the 00147 // distance and one for the angle, and perform transformations 00148 dist=sqrt( -2.0 * log(double(rand()) / double(RAND_MAX)) ); 00149 angle=2.0 * PI * (double(rand()) / double(RAND_MAX)); 00150 00151 // calculate and store first deviate and set flag 00152 storedDeviate=dist*cos(angle); 00153 deviateAvailable=true; 00154 00155 // calcaulate return second deviate 00156 return dist * sin(angle) * sigma + mu; 00157 } 00158 00159 // If a deviate is available from a previous call to this function, it is 00160 // returned, and the flag is set to false. 00161 else { 00162 deviateAvailable=false; 00163 return storedDeviate*sigma + mu; 00164 } 00165 } 00166 00167 bool isRGBCloud( const sensor_msgs::PointCloud2ConstPtr& cloud ) 00168 { 00169 sensor_msgs::PointCloud2::_fields_type::const_iterator i, end; 00170 00171 for( i = cloud->fields.begin(), end = cloud->fields.end(); i != end; ++i ) 00172 { 00173 if( i->name == "rgb" ) 00174 { 00175 return true; 00176 } 00177 } 00178 return false; 00179 } 00180 00181 double add_noise_to_point(double val) { 00182 00183 double val_noise; 00184 double sigma; // standard deviation 00185 00186 if (val!=0.0) { 00187 00188 sigma = (2.6610e-06*(val*val*nac*nac)) - (1.0787e-03*val*nac) + 2.7011; 00189 00190 val_noise = randn_notrig(val,sigma); // to milimeters 00191 00192 if (val_noise < min_val) val_noise = min_val; 00193 00194 double q = 2.1983; 00195 00196 val_noise = floor((val_noise-min_val)/q) * q + q/2 + min_val; 00197 00198 } else val_noise = 0; 00199 00200 return val_noise; 00201 00202 } 00203 00204 void cloudCallback(const PointCloud2ConstPtr& cloud) { 00205 00206 sensor_msgs::PointCloud2 cloud_out; 00207 00208 if (!isRGBCloud(cloud)) { 00209 00210 ROS_INFO_ONCE("First pointcloud received."); 00211 00212 //if (m_pub.getNumSubscribers() == 0) return; 00213 00214 pcl::PointCloud<pcl::PointXYZ> pointcloud; 00215 pcl::PointCloud<pcl::PointXYZ> pointcloud_n; 00216 pcl::fromROSMsg (*cloud, pointcloud); 00217 00218 pointcloud_n.header = pointcloud.header; 00219 00220 for (unsigned int i = 0; i < pointcloud.points.size(); ++i) { 00221 00222 double val = pointcloud.points[i].z * 1000.0; // depth value in milimeters 00223 00224 if (val > max_val) continue; 00225 if (val < min_val) continue; 00226 00227 double noise_val = add_noise_to_point(val); 00228 00229 if (add_noise) pointcloud.points[i].z = noise_val / 1000.0; 00230 00231 pointcloud_n.points.push_back(pointcloud.points[i]); 00232 00233 } 00234 00235 pcl::toROSMsg(pointcloud_n,cloud_out); 00236 00237 00238 } else { 00239 00240 ROS_INFO_ONCE("First RGB pointcloud received."); 00241 00242 if (m_pub.getNumSubscribers() == 0) return; 00243 00244 pcl::PointCloud<pcl::PointXYZRGB> pointcloud; 00245 pcl::PointCloud<pcl::PointXYZRGB> pointcloud_n; 00246 pcl::fromROSMsg (*cloud, pointcloud); 00247 00248 pointcloud_n.header = pointcloud.header; 00249 00250 for (unsigned int i = 0; i < pointcloud.points.size(); ++i) { 00251 00252 double val = pointcloud.points[i].z * 1000.0; // depth value in milimeters 00253 00254 if (val > max_val) continue; 00255 if (val < min_val) continue; 00256 00257 double noise_val = add_noise_to_point(val); 00258 00259 if (add_noise) pointcloud.points[i].z = noise_val / 1000.0; 00260 00261 pointcloud_n.points.push_back(pointcloud.points[i]); 00262 00263 } 00264 00265 pcl::toROSMsg(pointcloud_n,cloud_out); 00266 00267 00268 } 00269 00270 00271 m_pub.publish(cloud_out); 00272 00273 00274 } 00275 00276 00277 00278 int main(int argc, char** argv) { 00279 00280 ros::init(argc, argv, "add_noise_pcl"); 00281 00282 ROS_INFO("Starting kinect noise generator node..."); 00283 00284 ros::NodeHandle n; 00285 00286 ros::param::param<bool>("~add_noise",add_noise,true); 00287 ros::param::param<double>("~noise_amount_coef",nac,1.0); 00288 00289 ros::Subscriber sub; 00290 00291 m_pub = n.advertise<sensor_msgs::PointCloud2> ("points_out", 1); 00292 00293 sub = n.subscribe("points_in", 1, cloudCallback); 00294 00295 /*ros::Rate r(1); 00296 00297 ros::AsyncSpinner spinner(5); 00298 spinner.start();*/ 00299 00300 ROS_INFO("Spinning"); 00301 00302 /*while(ros::ok()) { 00303 00304 if (m_pub.getNumSubscribers() != 0) sub = n.subscribe("points_in", 1, cloudCallback); 00305 else sub.shutdown(); 00306 00307 r.sleep(); 00308 00309 } 00310 00311 spinner.stop();*/ 00312 00313 ros::spin(); 00314 00315 }