Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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();
- }
Add Comment
Please, Sign In to add comment