SHARE
TWEET

kinect - add noise

ZdenekM Jul 3rd, 2012 171 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ros/ros.h>
  2. #include <sensor_msgs/CameraInfo.h>
  3. #include <sensor_msgs/Image.h>
  4. #include <cv_bridge/cv_bridge.h>
  5. #include <opencv/cv.h>
  6. #include <opencv/cvwimage.h>
  7. //#include <cv_bridge/CvBridge.h>
  8. #include <opencv2/imgproc/imgproc.hpp>
  9. #include <opencv2/highgui/highgui.hpp>
  10. //#include <tf/transform_datatypes.h>
  11. #include <iostream>
  12. #include <fstream>
  13. #include <cmath>
  14. #include <image_transport/image_transport.h>
  15. #include <pcl_ros/point_cloud.h>
  16. #include <pcl/point_types.h>
  17. #include <sensor_msgs/image_encodings.h>
  18. #include <image_geometry/pinhole_camera_model.h>
  19. #include "depth_traits.h"
  20.  
  21. #include <message_filters/subscriber.h>
  22. #include <message_filters/synchronizer.h>
  23. #include <message_filters/time_synchronizer.h>
  24. #include <message_filters/sync_policies/exact_time.h>
  25. #include <message_filters/sync_policies/approximate_time.h>
  26.  
  27.  
  28. using namespace std;
  29.  
  30. static const char WINDOW_NAME[] = "Depth View";
  31.  
  32. image_transport::Publisher m_pub;
  33. bool add_noise = true;
  34.  
  35.  
  36.  
  37. /******************************************************************************/
  38. /* randn()
  39.  *
  40.  * Normally (Gaussian) distributed random numbers, using the Box-Muller
  41.  * transformation.  This transformation takes two uniformly distributed deviates
  42.  * within the unit circle, and transforms them into two independently
  43.  * distributed normal deviates.  Utilizes the internal rand() function; this can
  44.  * easily be changed to use a better and faster RNG.
  45.  *
  46.  * The parameters passed to the function are the mean and standard deviation of
  47.  * the desired distribution.  The default values used, when no arguments are
  48.  * passed, are 0 and 1 - the standard normal distribution.
  49.  *
  50.  *
  51.  * Two functions are provided:
  52.  *
  53.  * The first uses the so-called polar version of the B-M transformation, using
  54.  * multiple calls to a uniform RNG to ensure the initial deviates are within the
  55.  * unit circle.  This avoids making any costly trigonometric function calls.
  56.  *
  57.  * The second makes only a single set of calls to the RNG, and calculates a
  58.  * position within the unit circle with two trigonometric function calls.
  59.  *
  60.  * The polar version is generally superior in terms of speed; however, on some
  61.  * systems, the optimization of the math libraries may result in better
  62.  * performance of the second.  Try it out on the target system to see which
  63.  * works best for you.  On my test machine (Athlon 3800+), the non-trig version
  64.  * runs at about 3x10^6 calls/s; while the trig version runs at about
  65.  * 1.8x10^6 calls/s (-O2 optimization).
  66.  *
  67.  *
  68.  * Example calls:
  69.  * randn_notrig();      //returns normal deviate with mean=0.0, std. deviation=1.0
  70.  * randn_notrig(5.2,3.0);       //returns deviate with mean=5.2, std. deviation=3.0
  71.  *
  72.  *
  73.  * Dependencies - requires <cmath> for the sqrt(), sin(), and cos() calls, and a
  74.  * #defined value for PI.
  75.  */
  76.  
  77. /******************************************************************************/
  78. //      "Polar" version without trigonometric calls
  79. double randn_notrig(double mu=0.0, double sigma=1.0) {
  80.         static bool deviateAvailable=false;     //      flag
  81.         static float storedDeviate;                     //      deviate from previous calculation
  82.         double polar, rsquared, var1, var2;
  83.  
  84.         //      If no deviate has been stored, the polar Box-Muller transformation is
  85.         //      performed, producing two independent normally-distributed random
  86.         //      deviates.  One is stored for the next round, and one is returned.
  87.         if (!deviateAvailable) {
  88.  
  89.                 //      choose pairs of uniformly distributed deviates, discarding those
  90.                 //      that don't fall within the unit circle
  91.                 do {
  92.                         var1=2.0*( double(rand())/double(RAND_MAX) ) - 1.0;
  93.                         var2=2.0*( double(rand())/double(RAND_MAX) ) - 1.0;
  94.                         rsquared=var1*var1+var2*var2;
  95.                 } while ( rsquared>=1.0 || rsquared == 0.0);
  96.  
  97.                 //      calculate polar tranformation for each deviate
  98.                 polar=sqrt(-2.0*log(rsquared)/rsquared);
  99.  
  100.                 //      store first deviate and set flag
  101.                 storedDeviate=var1*polar;
  102.                 deviateAvailable=true;
  103.  
  104.                 //      return second deviate
  105.                 return var2*polar*sigma + mu;
  106.         }
  107.  
  108.         //      If a deviate is available from a previous call to this function, it is
  109.         //      returned, and the flag is set to false.
  110.         else {
  111.                 deviateAvailable=false;
  112.                 return storedDeviate*sigma + mu;
  113.         }
  114. }
  115.  
  116.  
  117. /******************************************************************************/
  118. //      Standard version with trigonometric calls
  119. #define PI 3.14159265358979323846
  120.  
  121. double randn_trig(double mu=0.0, double sigma=1.0) {
  122.         static bool deviateAvailable=false;     //      flag
  123.         static float storedDeviate;                     //      deviate from previous calculation
  124.         double dist, angle;
  125.  
  126.         //      If no deviate has been stored, the standard Box-Muller transformation is
  127.         //      performed, producing two independent normally-distributed random
  128.         //      deviates.  One is stored for the next round, and one is returned.
  129.         if (!deviateAvailable) {
  130.  
  131.                 //      choose a pair of uniformly distributed deviates, one for the
  132.                 //      distance and one for the angle, and perform transformations
  133.                 dist=sqrt( -2.0 * log(double(rand()) / double(RAND_MAX)) );
  134.                 angle=2.0 * PI * (double(rand()) / double(RAND_MAX));
  135.  
  136.                 //      calculate and store first deviate and set flag
  137.                 storedDeviate=dist*cos(angle);
  138.                 deviateAvailable=true;
  139.  
  140.                 //      calcaulate return second deviate
  141.                 return dist * sin(angle) * sigma + mu;
  142.         }
  143.  
  144.         //      If a deviate is available from a previous call to this function, it is
  145.         //      returned, and the flag is set to false.
  146.         else {
  147.                 deviateAvailable=false;
  148.                 return storedDeviate*sigma + mu;
  149.         }
  150. }
  151.  
  152. void imageDepthCallback(const sensor_msgs::ImageConstPtr& msg/*, const sensor_msgs::CameraInfoConstPtr& info_msg*/) {
  153.  
  154.         static bool callback_received = false;
  155.  
  156.  
  157.         cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
  158.  
  159.  
  160.         try {
  161.  
  162.                 //cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::TYPE_16UC1);
  163.                 cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::MONO8);
  164.  
  165.         }
  166.         catch (cv_bridge::Exception& e)
  167.         {
  168.                 ROS_ERROR("cv_bridge exception: %s", e.what());
  169.                 return;
  170.           }
  171.  
  172.           if (callback_received == false)
  173.           {
  174.  
  175.             ROS_INFO("First callback received");
  176.             callback_received = true;
  177.             if (add_noise==false) ROS_WARN("Node will run in dummy node - no noise will be added");
  178.  
  179.             cout << "Cols: " << cv_ptr->image.cols << ", rows: " << cv_ptr->image.rows << endl;
  180.  
  181.           }
  182.  
  183.  
  184.         uint16_t px = (uint16_t)(cv_ptr->image.cols)/2;
  185.         uint16_t py = (uint16_t)(cv_ptr->image.rows)/2;
  186.  
  187.         for(uint16_t y=0; y < ((uint16_t)(cv_ptr->image.rows)); y++)
  188.         for(uint16_t x=0; x < ((uint16_t)(cv_ptr->image.cols)); x++) {
  189.  
  190.                 uint16_t val =  cv_ptr->image.at<uint16_t>(y,x);
  191.  
  192.  
  193.                 uint16_t val_noise;
  194.                 double sigma; // standard deviation
  195.  
  196.                 sigma = 2.6610e-06*(double)(val*val)  - 1.0787e-03*(double)val +  2.7011e+00;
  197.  
  198.                 val_noise = (uint16_t)ceil(randn_notrig((double)val,sigma));
  199.  
  200.                 if (add_noise) cv_ptr->image.at<uint16_t>(y,x) = val_noise;
  201.  
  202.                 if (x==px && y==py) cout << "orig. value(" << px << "," << py << "): " << val << ", new value: " << cv_ptr->image.at<uint16_t>(y,x) << endl;
  203.  
  204.  
  205.                 }
  206.  
  207.         cv::imshow(WINDOW_NAME, cv_ptr->image);
  208.         cv::waitKey(3);
  209.  
  210.  
  211.         sensor_msgs::Image::Ptr noisy_msg = cv_ptr->toImageMsg();
  212.  
  213.  
  214.  
  215.         // publish
  216.         try {
  217.  
  218.                 m_pub.publish(noisy_msg);
  219.  
  220.         }
  221.  
  222.         catch (image_transport::Exception& e) {
  223.  
  224.                 ROS_ERROR("image_transport exception: %s", e.what());
  225.                 return;
  226.  
  227.         }
  228.  
  229. }
  230.  
  231.  
  232.  
  233. int main(int argc, char** argv) {
  234.  
  235.         ros::init(argc, argv, "add_noise");
  236.  
  237.         ROS_INFO("Starting node...");
  238.  
  239.         ros::NodeHandle n;
  240.  
  241.         cv::namedWindow(WINDOW_NAME);
  242.  
  243.         ros::param::param<bool>("~add_noise",add_noise,true);
  244.  
  245.         image_transport::ImageTransport it(n);
  246.         m_pub = it.advertise("depth_out", 1 );
  247.         image_transport::Subscriber sub_depth = it.subscribe("depth_in", 1, imageDepthCallback);
  248.  
  249.  
  250.         /*message_filters::Subscriber<sensor_msgs::Image> image_sub(n, "depth_in", 1);
  251.         message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub(n, "camera_info", 1);
  252.         typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
  253.         message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
  254.         sync.registerCallback(boost::bind(&imageDepthCallback, _1, _2));*/
  255.  
  256.         ROS_INFO("Spinning");
  257.  
  258.         ros::spin();
  259.  
  260. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top