00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "peddetect.h"
00028
00029 class ImageConverter
00030 {
00031 ros::NodeHandle nh_;
00032 image_transport::ImageTransport it_;
00033 image_transport::Subscriber image_sub_;
00034 image_transport::Publisher image_pub_;
00035 int ImgCount;
00036 cv_bridge::CvImagePtr cv_ptr;
00037 DVector features;
00038 CvRect region;
00039 Size minSize;
00040 FtrVecParams2 randparams;
00041 vector<DVector> WindowFtrs;
00042 RNG rng;
00043
00044 public:
00045
00046 ImageConverter ():
00047 it_ (nh_)
00048 {
00049
00050 ImgCount = 1;
00051 image_pub_ = it_.advertise ("Image_Out", 1);
00052 image_sub_ =
00053 it_.subscribe ("Image_In", 1, &ImageConverter::imageCb, this);
00054
00055 region.x = 0; region.y = 0; region.width = 64; region.height = 128;
00056 minSize.width=region.width; minSize.height=region.height;
00057
00058 GetRandParams(SEED,NRFEATURE2, randparams, region);
00059 rng(123);
00060
00061 }
00062
00063 ~ImageConverter ()
00064 {
00065
00066 }
00067
00068 void imageCb (const sensor_msgs::ImageConstPtr & msg)
00069 {
00070
00071 try
00072 {
00073
00074 cv_ptr = cv_bridge::toCvCopy (msg, "rgb8");
00075
00076 cv_ptr->image.convertTo (cv_ptr->image, CV_8UC1);
00077
00078 }
00079 catch (cv_bridge::Exception & e)
00080 {
00081 ROS_ERROR ("cv_bridge exception: %s", e.what ());
00082 return;
00083 }
00084 Mat Img = cv_ptr->image;
00085
00086 if (Img.rows == 0)
00087 return;
00088
00089 Size win; win.width=640; win.height=480;
00090 resize(Img, Img, win , 0, 0, INTER_LINEAR);
00091
00092 region.x = 0; region.y = 0; region.width = DWWIDTH; region.height = DWHEIGHT;
00093
00095
00096 cvtColor (Img, Img, CV_BGR2RGB, 0);
00097
00098 features.clear ();
00099 WindowFtrs.clear();
00100
00101 GetChnFtrsOverImagePyramid(Img , region , features, WindowFtrs ,0, minSize, 8 , randparams);
00102
00103 fstream outfile;
00104 outfile.open ("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/train_10000.csv", fstream::in | fstream::out | fstream::app);
00105
00106 int a=rng.uniform(0,WindowFtrs.size());
00107 int b=rng.uniform(0,WindowFtrs.size());
00108 int c=rng.uniform(0,WindowFtrs.size());
00109 int d=rng.uniform(0,WindowFtrs.size());
00110
00111 outfile<<"2,";
00112
00113 for (int i=0;i<NRFEATURE;i++)
00114 {
00115 if (i != NRFEATURE-1)
00116 outfile<<WindowFtrs[a][i]<<",";
00117 else
00118 outfile<<WindowFtrs[a][i];
00119 }
00120
00121 outfile<<endl;
00122 outfile<<"2,";
00123 for (int i=0;i<NRFEATURE;i++)
00124 {
00125 if (i != NRFEATURE-1)
00126 outfile<<WindowFtrs[b][i]<<",";
00127 else
00128 outfile<<WindowFtrs[b][i];
00129 }
00130
00131
00132 outfile<<endl;
00133 outfile<<"2,";
00134 for (int i=0;i<NRFEATURE;i++)
00135 {
00136 if (i != NRFEATURE-1)
00137 outfile<<WindowFtrs[c][i]<<",";
00138 else
00139 outfile<<WindowFtrs[c][i];
00140 }
00141
00142 outfile<<endl;
00143 outfile<<"2,";
00144
00145 for (int i=0;i<NRFEATURE;i++)
00146 {
00147 if (i != NRFEATURE-1)
00148 outfile<<WindowFtrs[d][i]<<",";
00149 else
00150 outfile<<WindowFtrs[d][i];
00151 }
00152
00153 outfile<<endl;
00154
00155
00156 outfile.close();
00157 ROS_INFO ("Image nr %d\n", ImgCount++);
00158
00159
00160
00161
00162
00164
00165
00166 cv_ptr->image = Img;
00167
00168
00169
00170 sensor_msgs::ImagePtr msg_out = cv_ptr->toImageMsg ();
00171 msg_out->encoding = "rgb8";
00172 msg_out->header.frame_id = "Image_Out";
00173 msg_out->header.stamp = ros::Time::now ();
00174
00175
00176 image_pub_.publish (msg_out);
00177
00178
00179
00180 }
00181
00182 };
00183
00184 int main (int argc, char **argv)
00185 {
00186 fstream outfile("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/train_10000.csv");
00187 ros::init (argc, argv, "image_converter");
00188 ImageConverter ic;
00189 ros::spin ();
00190 return 0;
00191 }