filewriter.cpp

Go to the documentation of this file.

/*
 * detector.cpp
 *
 *  Created on: Mar 8, 2011
 *      Author: sturm
 */

#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <tf/transform_datatypes.h>
#include <articulation_msgs/TrackMsg.h>
#include <fstream>

using namespace std;

bool rgb=false;
string fname;
sensor_msgs::CameraInfo::ConstPtr cameraInfo_;
cv_bridge::CvImagePtr cv_ptr;
cv::Mat debugImage_;


inline cv::Point3f to3D(cv::Point a) {
        cv::Point3f pt;
        float constant = 1.00 / cameraInfo_->K[0];
        pt.x = (a.x - (cv_ptr->image.cols >> 1)) * cv_ptr->image.at<float> (a)
                        * constant;
        pt.y = (a.y - (cv_ptr->image.rows >> 1)) * cv_ptr->image.at<float> (a)
                        * constant;

        pt.z = cv_ptr->image.at<float> (a);
        return pt;
}

inline cv::Point to2D(cv::Point3f pt) {
        cv::Point a;
        a.x = pt.x/ pt.z * cameraInfo_->K[0] +  (cv_ptr->image.cols >> 1);
        a.y = pt.y/ pt.z * cameraInfo_->K[0] +  (cv_ptr->image.rows >> 1);
        return a;
}


void infoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) {
        cameraInfo_ = msg;
        cout << "received info" << endl;
}

void imageCallback(const sensor_msgs::Image::ConstPtr& msg) {
        cout << "received image" << endl;
        ros::Time begin = ros::Time::now();
        if (!cameraInfo_)
                return;
        try {
                cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO16);
        } catch (cv_bridge::Exception& e) {
                ROS_ERROR("cv_bridge exception: %s", e.what());
                return;
        }

//      debugImage_ = cv_ptr->image * 0.2;
        cvtColor(cv_ptr->image * 0.2, debugImage_, CV_GRAY2BGR);

        cv::imshow("depth", debugImage_);

        fstream stream((fname + ".txt").c_str(),ios::out);
        stream <<"# u v x y z"<<endl;
        for(int y=0;y<cv_ptr->image.rows;y++) {
                for(int x=0;x<cv_ptr->image.cols;x++) {
                        cv::Point3f p = to3D(cv::Point(x,y));
                        if(isfinite(p.x))
                                stream <<x<<"\t"<<y<<"\t"<<p.x<<"\t"<<p.y<<"\t"<<p.z<<endl;
                }
        }
        stream.close();
        cv::imwrite(fname + "-depth.png", debugImage_*256);

        fstream stream2((fname + "-depth.txt").c_str(),ios::out);
        stream <<"# per pixel depth in meter, cols x rows ="<<cv_ptr->image.cols<<"x"<<cv_ptr->image.rows<<endl;
        for(int y=0;y<cv_ptr->image.rows;y++) {
                for(int x=0;x<cv_ptr->image.cols;x++) {
                        cv::Point p = cv::Point(x,y);
                        stream2 << cv_ptr->image.at<float> (p)<<" ";
                }
                stream2 << endl;
        }
        stream2.close();

        if(rgb)
                exit(0);
//      cv::waitKey(3000);
}

void imageRGBCallback(const sensor_msgs::Image::ConstPtr& msg) {
        cout << "received RGB image" << endl;
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        cv::cvtColor(cv_ptr->image,cv_ptr->image,CV_RGB2BGR);
        cv::imwrite(fname+"-color.png",cv_ptr->image);

        fstream stream((fname + "-color.txt").c_str(),ios::out);
        stream <<"# pixel-wise (r g b), cols x rows ="<<cv_ptr->image.cols<<"x"<<cv_ptr->image.rows<<endl;
        for(int y=0;y<cv_ptr->image.rows;y++) {
                for(int x=0;x<cv_ptr->image.cols;x++) {
                        cv::Point p = cv::Point(x,y);
                        stream << (int)cv_ptr->image.at<cv::Vec3b>(p)[2] << " ";
                        stream << (int)cv_ptr->image.at<cv::Vec3b>(p)[1] << " ";
                        stream << (int)cv_ptr->image.at<cv::Vec3b>(p)[0] << " ";
                }
                stream << endl;
        }
        stream.close();

        rgb=true;
}

int main(int argc, char** argv) {
        ros::init(argc, argv, "filewriter");

        if(argc != 2) {
                cout <<"wrong number of arguments. Usage: filewriter <prefix>"<<endl;
                exit(0);
        }
        fname = string("kinect-")+argv[1];

        ros::NodeHandle n;

        ros::Subscriber sub = n.subscribe("camera/depth/image", 1, imageCallback);
        ros::Subscriber sub3 = n.subscribe("camera/rgb/image_color", 1, imageRGBCallback);
        ros::Subscriber sub2 = n.subscribe("camera/depth/camera_info", 1,
                        infoCallback);

        cv::namedWindow("depth");

        ros::spin();
}