kinect - add noise, 2nd iteration

#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv/cv.h>
#include <opencv/cvwimage.h>
//#include <cv_bridge/CvBridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
//#include <tf/transform_datatypes.h>
#include <iostream>
#include <fstream>
#include <cmath>
#include <image_transport/image_transport.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>


using namespace std;

static const char WINDOW_NAME[] = "Depth View";

image_transport::Publisher m_pub;
bool add_noise = true;



/******************************************************************************/
/* randn()
 *
 * Normally (Gaussian) distributed random numbers, using the Box-Muller
 * transformation.  This transformation takes two uniformly distributed deviates
 * within the unit circle, and transforms them into two independently
 * distributed normal deviates.  Utilizes the internal rand() function; this can
 * easily be changed to use a better and faster RNG.
 *
 * The parameters passed to the function are the mean and standard deviation of
 * the desired distribution.  The default values used, when no arguments are
 * passed, are 0 and 1 - the standard normal distribution.
 *
 *
 * Two functions are provided:
 *
 * The first uses the so-called polar version of the B-M transformation, using
 * multiple calls to a uniform RNG to ensure the initial deviates are within the
 * unit circle.  This avoids making any costly trigonometric function calls.
 *
 * The second makes only a single set of calls to the RNG, and calculates a
 * position within the unit circle with two trigonometric function calls.
 *
 * The polar version is generally superior in terms of speed; however, on some
 * systems, the optimization of the math libraries may result in better
 * performance of the second.  Try it out on the target system to see which
 * works best for you.  On my test machine (Athlon 3800+), the non-trig version
 * runs at about 3x10^6 calls/s; while the trig version runs at about
 * 1.8x10^6 calls/s (-O2 optimization).
 *
 *
 * Example calls:
 * randn_notrig();  //returns normal deviate with mean=0.0, std. deviation=1.0
 * randn_notrig(5.2,3.0);   //returns deviate with mean=5.2, std. deviation=3.0
 *
 *
 * Dependencies - requires <cmath> for the sqrt(), sin(), and cos() calls, and a
 * #defined value for PI.
 */

/******************************************************************************/
//  "Polar" version without trigonometric calls
double randn_notrig(double mu=0.0, double sigma=1.0) {
    static bool deviateAvailable=false; //  flag
    static float storedDeviate;         //  deviate from previous calculation
    double polar, rsquared, var1, var2;

    //  If no deviate has been stored, the polar Box-Muller transformation is
    //  performed, producing two independent normally-distributed random
    //  deviates.  One is stored for the next round, and one is returned.
    if (!deviateAvailable) {

        //  choose pairs of uniformly distributed deviates, discarding those
        //  that don't fall within the unit circle
        do {
            var1=2.0*( double(rand())/double(RAND_MAX) ) - 1.0;
            var2=2.0*( double(rand())/double(RAND_MAX) ) - 1.0;
            rsquared=var1*var1+var2*var2;
        } while ( rsquared>=1.0 || rsquared == 0.0);

        //  calculate polar tranformation for each deviate
        polar=sqrt(-2.0*log(rsquared)/rsquared);

        //  store first deviate and set flag
        storedDeviate=var1*polar;
        deviateAvailable=true;

        //  return second deviate
        return var2*polar*sigma + mu;
    }

    //  If a deviate is available from a previous call to this function, it is
    //  returned, and the flag is set to false.
    else {
        deviateAvailable=false;
        return storedDeviate*sigma + mu;
    }
}


/******************************************************************************/
//  Standard version with trigonometric calls
#define PI 3.14159265358979323846

double randn_trig(double mu=0.0, double sigma=1.0) {
    static bool deviateAvailable=false; //  flag
    static float storedDeviate;         //  deviate from previous calculation
    double dist, angle;

    //  If no deviate has been stored, the standard Box-Muller transformation is
    //  performed, producing two independent normally-distributed random
    //  deviates.  One is stored for the next round, and one is returned.
    if (!deviateAvailable) {

        //  choose a pair of uniformly distributed deviates, one for the
        //  distance and one for the angle, and perform transformations
        dist=sqrt( -2.0 * log(double(rand()) / double(RAND_MAX)) );
        angle=2.0 * PI * (double(rand()) / double(RAND_MAX));

        //  calculate and store first deviate and set flag
        storedDeviate=dist*cos(angle);
        deviateAvailable=true;

        //  calcaulate return second deviate
        return dist * sin(angle) * sigma + mu;
    }

    //  If a deviate is available from a previous call to this function, it is
    //  returned, and the flag is set to false.
    else {
        deviateAvailable=false;
        return storedDeviate*sigma + mu;
    }
}

void imageDepthCallback(const sensor_msgs::ImageConstPtr& msg/*, const sensor_msgs::CameraInfoConstPtr& info_msg*/) {

    static bool callback_received = false;


    cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);

    sensor_msgs::ImagePtr msg_c(new sensor_msgs::Image);

    msg_c->data = msg->data;
    msg_c->encoding = msg->encoding;
    msg_c->header = msg->header;
    msg_c->height = msg->height;
    msg_c->width = msg->width;
    msg_c->is_bigendian = msg->is_bigendian;
    msg_c->step = msg->step;

    if (msg_c->encoding=="mono8") {

        msg_c->encoding = "16UC1";
        msg_c->step = 1280;

    }


    try {

        //cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::TYPE_16UC1);
        //cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::MONO8);
        cv_ptr = cv_bridge::toCvCopy(msg_c/*,sensor_msgs::image_encodings::MONO8*/);

    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
      }

      if (callback_received == false)
      {

        ROS_INFO("First callback received");
        callback_received = true;
        if (add_noise==false) ROS_WARN("Node will run in dummy node - no noise will be added");

        cout << "Cols: " << cv_ptr->image.cols << ", rows: " << cv_ptr->image.rows << endl;

      }


    uint16_t px = (uint16_t)(cv_ptr->image.cols)/2;
    uint16_t py = (uint16_t)(cv_ptr->image.rows)/2;

    for(uint16_t y=0; y < ((uint16_t)(cv_ptr->image.rows)); y++)
    for(uint16_t x=0; x < ((uint16_t)(cv_ptr->image.cols)); x++) {

        uint16_t val =  cv_ptr->image.at<uint16_t>(y,x);


        uint16_t val_noise;
        double sigma; // standard deviation

        if (val!=0) {

            sigma = 2.6610e-06*(double)(val*val)  - 1.0787e-03*(double)val +  2.7011e+00;
            val_noise = (uint16_t)ceil(randn_notrig((double)val,sigma));

            if (add_noise) cv_ptr->image.at<uint16_t>(y,x) = val_noise;

        } else val_noise = 0;



        //if (x==px && y==py) cout << "orig. value(" << px << "," << py << "): " << val << ", new value: " << cv_ptr->image.at<uint16_t>(y,x) << endl;


        }

    cv_ptr->image.convertTo(cv_ptr->image,CV_32FC1);

    cv_ptr->image *= 0.001;

    cv_ptr->encoding = "32FC1";

    cv::imshow(WINDOW_NAME, cv_ptr->image);
    cv::waitKey(3);


    sensor_msgs::Image::Ptr noisy_msg = cv_ptr->toImageMsg();



    // publish
    try {

        m_pub.publish(noisy_msg);

    }

    catch (image_transport::Exception& e) {

        ROS_ERROR("image_transport exception: %s", e.what());
        return;

    }

}



int main(int argc, char** argv) {

    ros::init(argc, argv, "add_noise");

    ROS_INFO("Starting node...");

    ros::NodeHandle n;

    cv::namedWindow(WINDOW_NAME);

    ros::param::param<bool>("~add_noise",add_noise,true);

    image_transport::ImageTransport it(n);
    m_pub = it.advertise("depth_out", 1 );
    image_transport::Subscriber sub_depth = it.subscribe("depth_in", 1, imageDepthCallback);


    /*message_filters::Subscriber<sensor_msgs::Image> image_sub(n, "depth_in", 1);
    message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub(n, "camera_info", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
    sync.registerCallback(boost::bind(&imageDepthCallback, _1, _2));*/

    ROS_INFO("Spinning");

    ros::spin();

}