swissranger_viewer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* author: Radu Bogdan Rusu <rusu@cs.tum.edu> */
00031 
00032 #include <cstdio>
00033 #include <vector>
00034 #include <opencv/cv.h>
00035 #include <opencv/highgui.h>
00036 #include <ros/node_handle.h>
00037 #include <deprecated_msgs/ImageArray.h>
00038 
00039 
00040 
00041 extern "C" {
00046   typedef struct AVPicture {
00047     uint8_t *data[4];
00048     int linesize[4];       
00049   } AVPicture;
00050   
00051   
00052   
00053            // Uncomment this macro for using a video input file
00054   int img_convert(AVPicture *dst, int dst_pix_fmt,
00055                   const AVPicture *src, int src_pix_fmt,
00056                   int src_width, int src_height){
00057     return(0);
00058     
00059   }
00060 }
00061 
00062 
00063 using namespace std;
00064 using namespace deprecated_msgs;
00065 
00066 class SwissRangerViewer
00067 {
00068   public:
00069   ros::NodeHandle node_;
00070   ros::Subscriber sr_sub_ ;
00071   //public:
00072   //    ImageArray images_sr;
00073     char key;
00074     int imgnum;
00075     IplImage *sr_img1, *sr_img2, *sr_img3;
00076   
00077   //SwissRangerViewer (ros::NodeHandle& anode) : node_ (anode)
00078   SwissRangerViewer ()
00079     {
00080       cvNamedWindow ("sr4k - distance", CV_WINDOW_AUTOSIZE);
00081       cvNamedWindow ("sr4k - amplitude", CV_WINDOW_AUTOSIZE);
00082       cvNamedWindow ("sr4k - confidence", CV_WINDOW_AUTOSIZE);
00083       sr_sub_ = node_.subscribe<deprecated_msgs::ImageArray>
00084       ("/swissranger_test/images_sr", 10, &SwissRangerViewer::image_sr_cb, this);
00085       sr_img1 = cvCreateImage (cvSize (1, 1), IPL_DEPTH_16U, 1);
00086       sr_img2 = cvCreateImage (cvSize (1, 1), IPL_DEPTH_16U, 1);
00087       sr_img3 = cvCreateImage (cvSize (1, 1), IPL_DEPTH_16U, 1);
00088       imgnum = 0;
00089     }
00090 
00091   void image_sr_cb(const deprecated_msgs::ImageArray::ConstPtr &images_sr)
00092     {
00093       cvReleaseImage (&sr_img1);
00094       cvReleaseImage (&sr_img2);
00095       cvReleaseImage (&sr_img3);
00096 
00097       fprintf (stderr, "Received %d images of size:", images_sr->get_images_size ());
00098       for (unsigned int j = 0; j < images_sr->get_images_size (); j++)
00099       {
00100         fprintf (stderr, " %dx%d and type %s,", images_sr->images[j].width, images_sr->images[j].height, images_sr->images[j].colorspace.c_str ());
00101         if (images_sr->images[j].width == 0 || images_sr->images[j].height == 0)
00102          return;
00103       }
00104       fprintf (stderr, "\n");
00105 
00106       sr_img1 = cvCreateImage (cvSize (images_sr->images[0].width, images_sr->images[0].height), IPL_DEPTH_16U, 1);
00107       sr_img2 = cvCreateImage (cvSize (images_sr->images[1].width, images_sr->images[1].height), IPL_DEPTH_16U, 1);
00109       sr_img3 = cvCreateImage (cvSize (images_sr->images[2].width, images_sr->images[2].height), IPL_DEPTH_16U, 1);
00110 
00111       memcpy (sr_img1->imageData, &(images_sr->images[0].data[0]), sr_img1->imageSize);
00112       memcpy (sr_img2->imageData, &(images_sr->images[1].data[0]), sr_img2->imageSize);
00113       memcpy (sr_img3->imageData, &(images_sr->images[2].data[0]), sr_img3->imageSize);
00114 
00115       cvShowImage ("sr4k - distance", sr_img1);
00116 
00117       IplImage* sr_img2_rot = cvCloneImage (sr_img2);
00118       sr_img2 = cvCreateImage (cvGetSize (sr_img2_rot), IPL_DEPTH_8U, 3);
00119       if (sr_img2_rot->depth == IPL_DEPTH_16U)
00120         cvConvertImage (sr_img2_rot, sr_img2, CV_CVTIMG_SWAP_RB);
00121 
00122       IplImage *sr_img2_a = cvCreateImage (cvGetSize (sr_img2_rot), IPL_DEPTH_8U, 1);
00123       cvCvtColor (sr_img2, sr_img2_a, CV_RGB2GRAY);
00124         
00125       IplImage *sr_img2_rot_hist = cvCloneImage (sr_img2_a);
00126       cvEqualizeHist (sr_img2_a, sr_img2_rot_hist);
00127 
00128       cvReleaseImage (&sr_img2);
00129         
00130       sr_img2 = cvCreateImage (cvSize (sr_img2_rot_hist->width << 1, sr_img2_rot_hist->height << 1), IPL_DEPTH_8U, 1);
00131       cvResize (sr_img2_rot_hist, sr_img2, CV_INTER_NN);
00132       cvShowImage ("sr4k - amplitude", sr_img2);
00133               
00134       cvReleaseImage (&sr_img2_rot_hist);
00135       cvReleaseImage (&sr_img2_rot);
00136 
00137       cvShowImage ("sr4k - confidence", sr_img3);
00138       cvWaitKey (20);
00139     }
00140 
00141 };
00142 
00143 int
00144   main (int argc, char **argv)
00145 {
00146   ros::init (argc, argv, "swissranger_viewer");
00147   //ros::NodeHandle nh("~");
00148   SwissRangerViewer n;
00149   ros::spin ();
00150 
00151   return (0);
00152 }


swissranger_visualizer
Author(s): Radu Bogdan Rusu (rusu@cs.tum.edu)
autogenerated on Mon Oct 6 2014 08:03:58