SHARE
TWEET

kinect - add noise, 2nd iteration

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