00001 #include <ros/ros.h>
00002
00003 #include "peddetect.h"
00004
00005 #include "mtt/TargetList.h"
00006
00007 #include "cmath"
00008 #include <iostream>
00009 #include <vector>
00010
00011 #include <image_transport/image_transport.h>
00012
00013 #include <cv_bridge/cv_bridge.h>
00014 #include <opencv2/imgproc/imgproc.hpp>
00015 #include <opencv2/highgui/highgui.hpp>
00016 #include <opencv2/opencv.hpp>
00017 #include <opencv/cvwimage.h>
00018
00019 #include <visualization_msgs/Marker.h>
00020 #include <visualization_msgs/MarkerArray.h>
00021
00022 #include "sensor_msgs/LaserScan.h"
00023 #include "sensor_msgs/PointCloud.h"
00024 #include <sensor_msgs/image_encodings.h>
00025
00026 #include <boost/thread.hpp>
00027 #include <boost/shared_ptr.hpp>
00028
00029 #include <sensor_msgs/PointCloud2.h>
00030 #include <pcl/conversions.h>
00031 #include <pcl/point_cloud.h>
00032 #include <pcl/point_types.h>
00033
00034 #include <pcl/io/pcd_io.h>
00035
00036 #include <pcl/PCLPointCloud2.h>
00037 #include <pcl_conversions/pcl_conversions.h>
00038 #include <pcl/pcl_base.h>
00039
00040 using namespace std;
00041 using namespace cv;
00042 namespace enc = sensor_msgs::image_encodings;
00043
00044
00045
00046
00047 typedef enum
00048 {
00049 PEDESTRIAN,
00050 NON_PEDESTRIAN,
00051 UNKNOWN
00052 } TypeClassification;
00053
00054 class BoostClassifier
00055 {
00056 public:
00057 typedef boost::shared_ptr<BoostClassifier> Ptr;
00058
00059 bool load(const boost::filesystem::path& classifier_file)
00060 {
00061 region.x = 0;
00062 region.y = 0;
00063
00064 region.width = DWWIDTH;
00065 region.height = DWHEIGHT;
00066
00067 GetRandParams(SEED,NRFEATURE2, randparams, region);
00068 boost_classifier.load(classifier_file.string().c_str());
00069 }
00070
00071 CvRect region;
00072 FtrVecParams2 classifier_randparams;
00073 CvBoost classifier_boost;
00074 FtrVecParams2 randparams;
00075 CvBoost boost_classifier;
00076 };
00077
00078 class PedestrianClassifier
00079 {
00080 public:
00081 PedestrianClassifier(const BoostClassifier::Ptr& classifier_base_):
00082 classifier_base(classifier_base_)
00083 {
00084 region.x = 0;
00085 region.y = 0;
00086
00087 region.width = DWWIDTH;
00088 region.height = DWHEIGHT;
00089
00090 minSize.width = DWWIDTH;
00091 minSize.height = DWHEIGHT;
00092 }
00093
00094 TypeClassification classify(const Mat& image)
00095 {
00096 GetChnFtrsOverImagePyramid(image, region, features ,NOCTUP, minSize, SCALEPOCT ,classifier_base->randparams,PedRect, classifier_base->boost_classifier);
00097
00098 if(PedRect.size()>1)
00099 return PEDESTRIAN;
00100 else
00101 return NON_PEDESTRIAN;
00102 }
00103
00104 private:
00105
00106 BoostClassifier::Ptr classifier_base;
00107
00108 CvRect region;
00109 Size minSize;
00110
00111 DVector features;
00112 PedRoiVec PedRect;
00113 };
00114
00115 class Candidate
00116 {
00117 public:
00118
00119 typedef boost::shared_ptr<Candidate> Ptr;
00120 long int id;
00121 double distance;
00122 bool marked_for_deletion;
00123 bool classification_busy;
00124
00125 Point3f world_upper_left;
00126 Point3f world_down_right;
00127
00128 Point2f image_upper_right;
00129 Point2f image_down_left;
00130
00131 boost::thread object_thread;
00132 TypeClassification classification;
00133 BoostClassifier::Ptr classifier_base;
00134
00135
00136 Mat object_image;
00137
00138 Candidate(const BoostClassifier::Ptr& classifier_base_):
00139 classifier_base(classifier_base_)
00140 {
00141
00142 }
00143
00144
00145 bool projectToImage(const Mat& rvecs_, const Mat& tvecs_, const Mat& camera_matrix, const Mat& dist_coeffs)
00146 {
00147 vector<Point3f> world_points;
00148 vector<Point2f> image_points;
00149
00150
00151 world_points.push_back(world_upper_left);
00152 world_points.push_back(world_down_right);
00153
00154 try
00155 {
00156 cv::projectPoints(world_points, rvecs_, tvecs_, camera_matrix, dist_coeffs, image_points);
00157 }
00158 catch(cv::Exception& e)
00159 {
00160 cout << "ERROR: " << e.what() << endl;
00161 return false;
00162 }
00163
00164 image_upper_right = Point2f(image_points[0].x + 50, image_points[0].y - 950*tan(1./distance));
00165 image_down_left = Point2f(image_points[1].x - 50, image_points[1].y + 1050*tan(1./distance));
00166
00167 return true;
00168 }
00169
00170 bool crop(const Mat& image)
00171 {
00172 int x = image_down_left.x;
00173 int y = image_upper_right.y;
00174 int width = image_upper_right.x - image_down_left.x;
00175 int height = image_down_left.y - image_upper_right.y;
00176
00177
00178 if(x<0)
00179 x=0;
00180 if(x+width > image.cols)
00181 width=(image.cols-x);
00182
00183 if(y<0)
00184 y=0;
00185 if(y+height > image.rows)
00186 height=(image.rows-y);
00187
00188 try
00189 {
00190 object_image = image(Rect(x,y,width,height));
00191 }
00192 catch(cv::Exception& e)
00193 {
00194 cout << "ERROR: " << e.what() << endl;
00195 }
00196
00197 return true;
00198 }
00199
00200 bool draw(Mat& image)
00201 {
00202 if (classification == PEDESTRIAN)
00203 {
00204 putText(image, "PEDESTRIAN", Point2f(image_down_left.x,image_upper_right.y), FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(0,255,0), 1, CV_AA);
00205 rectangle(image, image_upper_right, image_down_left, Scalar(0,255,0), 2);
00206 }
00207 else
00208 {
00209 if(classification == NON_PEDESTRIAN)
00210 {
00211 putText(image, "OBJECT", Point2f(image_down_left.x,image_upper_right.y), FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(0,0,250), 1, CV_AA);
00212 }
00213 else
00214 {
00215 if(classification_busy == true)
00216 {
00217 putText(image, "PROCESSING", Point2f(image_down_left.x,image_upper_right.y), FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(0,0,250), 1, CV_AA);
00218 }
00219 else
00220 {
00221 putText(image, "NOT PROCESSED", Point2f(image_down_left.x,image_upper_right.y), FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(0,0,250), 1, CV_AA);
00222 }
00223 }
00224 rectangle(image, image_upper_right, image_down_left, Scalar(0,0,255), 2);
00225 }
00226
00227 return true;
00228 }
00229
00230 void classify()
00231 {
00232 object_thread = boost::thread(boost::bind(&Candidate::doClassification,this));
00233 }
00234
00235 void doClassification()
00236 {
00237 classification_busy = true;
00238 double secs = ros::Time::now().toSec();
00239
00240 PedestrianClassifier classifier(classifier_base);
00241
00242
00243 classification = classifier.classify(object_image);
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253 classification_busy = false;
00254 }
00255
00256 bool isBusy()
00257 {
00258 if (classification_busy)
00259 {
00260 return true;
00261 }
00262 else
00263 {
00264 return false;
00265 }
00266 }
00267
00268 bool kill()
00269 {
00270 object_thread.interrupt();
00271 object_thread.join();
00272 return true;
00273 }
00274
00275 };
00276
00277 class ImageConverter
00278 {
00279 ros::NodeHandle nh_;
00280 image_transport::ImageTransport it_;
00281 image_transport::Subscriber image_sub_;
00282 ros::Subscriber cam_info_sub_;
00283 ros::Subscriber mtt_laser_sub_;
00284 ros::Subscriber initial_laser_sub_;
00285
00286 ros::Subscriber extrinsic_sub_;
00287
00288 image_transport::Publisher image_pub_;
00289
00290 public:
00291
00292
00293 uint id;
00294 uint priority;
00295 double distance_to_target_from_center_bumper;
00296 double theta;
00297
00298 cv::Mat cameraMatrix;
00299 cv::Mat distCoeffs;
00300 cv::Mat rvecs;
00301 cv::Mat tvecs;
00302 cv::Mat rotation_matriz;
00303 cv::Mat trans_matriz;
00304
00305 int number_of_threads;
00306
00307 Mat final_image;
00308 vector<cv::Point3f> laser_points;
00309 vector<Candidate::Ptr> candidates;
00310
00311 BoostClassifier::Ptr classifier_base;
00312
00313 ImageConverter()
00314 : it_(nh_),
00315 classifier_base(new BoostClassifier)
00316 {
00317
00318 image_pub_ = it_.advertise("/out", 1);
00319
00320 image_sub_ = it_.subscribe("/usb_cam_reader/image_color", 1, &ImageConverter::imageCb, this);
00321
00322 cam_info_sub_ = nh_.subscribe("/usb_cam_reader/camera_info", 1, &ImageConverter::cameraInfoCallback, this);
00323
00324 mtt_laser_sub_ = nh_.subscribe("/targets", 1, &ImageConverter::mttLaserGather, this);
00325 initial_laser_sub_ = nh_.subscribe("/scan_cloud", 1, &ImageConverter::initiallaserGather, this);
00326
00327 string classifier_file;
00328
00329
00330
00331 classifier_file = "/home/asus/workingcopies/lar4/src/perception/pedestrians/multimodal_pedestrian_detect/trained_boost_15Kf_2000w_19Ks_m8_M64_boot3.xml" ;
00332
00333
00334
00335
00336 classifier_base->load(classifier_file);
00337
00338 number_of_threads = 0;
00339
00340 }
00341
00342 ~ImageConverter()
00343 {
00344
00345
00346 }
00347
00348 void cameraInfoCallback(const sensor_msgs::CameraInfo& cam_info_msg)
00349 {
00350
00351
00352 cameraMatrix = cvCreateMat(3,3, CV_64FC1);
00353 for (int i = 0; i < 3; i++)
00354 {
00355 for (int j = 0; j < 3; j++)
00356 {
00357 if (i==0){ cameraMatrix.at<double>(i, j) = cam_info_msg.P[3*i+j];}
00358 if (i==1){ cameraMatrix.at<double>(i, j) = cam_info_msg.P[3*i+j+1];}
00359 if (i==2){ cameraMatrix.at<double>(i, j) = cam_info_msg.P[3*i+j+2];}
00360 }
00361 }
00362
00363 distCoeffs = cvCreateMat(1,5 , CV_64FC1);
00364 for (int i = 0; i < 5; i++)
00365 distCoeffs.at<double>(0,i) = cam_info_msg.D[i];
00366
00367 cv::Mat rotation_matriz= cvCreateMat(3,1 , CV_64FC1);
00368 for (int i = 0; i < 3; i++)
00369 {
00370 rotation_matriz.at<double>(i,0) = cam_info_msg.R[i];
00371 }
00372
00373 rvecs= rotation_matriz;
00374
00375 cv::Mat trans_matriz = cvCreateMat(3,1, CV_64FC1);
00376 for (int i = 0; i < 3; i++)
00377 {
00378 trans_matriz.at<double>(i,0) = cam_info_msg.P[4*(i+1)-1];
00379 }
00380
00381 tvecs = trans_matriz;
00382
00383 cam_info_sub_.shutdown();
00384 }
00385
00386 void imageCb(const sensor_msgs::ImageConstPtr& cam_msg)
00387 {
00388 cv_bridge::CvImagePtr cv_ptr;
00389 try
00390 {
00391 cv_ptr = cv_bridge::toCvCopy(cam_msg, enc::BGR8);
00392 }
00393 catch (cv_bridge::Exception& e)
00394 {
00395 ROS_ERROR("cv_bridge exception: %s", e.what());
00396 return;
00397 }
00398
00399 if(tvecs.rows!=3)
00400 {
00401 cout<<"Incorrect size on T vecs matrix"<<endl;
00402 return;
00403 }
00404
00405
00406 std::vector<cv::Point2f> projectedPoints;
00407
00408 try
00409 {
00410 cv::projectPoints(laser_points, rvecs, tvecs, cameraMatrix,distCoeffs,projectedPoints);
00411 }
00412 catch(cv::Exception& e)
00413 {
00414 cout << "ERROR: " << e.what() << endl;
00415 }
00416
00417
00418 for(uint i=0;i<projectedPoints.size();i++)
00419 {
00420 line(cv_ptr->image, projectedPoints[i],projectedPoints[i],Scalar(0,0,255),5);
00421 }
00422
00423
00424 Mat cutted_image;
00425 cvtColor (cv_ptr->image, cutted_image, CV_BGR2RGB);
00426
00427 for(uint i=0;i<candidates.size();i++)
00428 if (candidates[i]->classification == UNKNOWN)
00429 if (candidates[i]->classification_busy == false)
00430 candidates[i]->crop(cutted_image);
00431
00432 for(uint i=0;i<candidates.size();i++)
00433 if (candidates[i]->classification == UNKNOWN)
00434 if (candidates[i]->classification_busy == false)
00435 candidates[i]->classify();
00436
00437 for(uint i=0;i<candidates.size();i++)
00438 candidates[i]->draw(cv_ptr->image);
00439
00440 imshow("test",cv_ptr->image);
00441 waitKey(2);
00442 }
00443
00444 void mttLaserGather(const mtt::TargetList& laser_msg)
00445 {
00446 double distance_to_target_from_center_bumper;
00447 double theta;
00448 double largura;
00449 bool id_found;
00450
00451
00452
00453 for(uint i=0;i<candidates.size();i++)
00454 {
00455 candidates[i]->marked_for_deletion = true;
00456 }
00457
00458 for(int i=0; i<laser_msg.Targets.size();i++)
00459 {
00460 id_found = false;
00461
00462 distance_to_target_from_center_bumper = sqrt(laser_msg.Targets[i].pose.position.x * laser_msg.Targets[i].pose.position.x +
00463 laser_msg.Targets[i].pose.position.y * laser_msg.Targets[i].pose.position.y);
00464
00465 theta = acos(laser_msg.Targets[i].pose.position.x / distance_to_target_from_center_bumper);
00466
00467 largura = sqrt(((laser_msg.Targets[i].finalpose.x - laser_msg.Targets[i].initialpose.x) * (laser_msg.Targets[i].finalpose.x - laser_msg.Targets[i].initialpose.x)) + ((laser_msg.Targets[i].finalpose.y - laser_msg.Targets[i].initialpose.y) * (laser_msg.Targets[i].finalpose.y - laser_msg.Targets[i].initialpose.y)));
00468
00469 if (theta < 0.45 && distance_to_target_from_center_bumper > 3.5 && largura > 0.15 && largura < 0.70 )
00470 {
00471 for(uint l=0;l<candidates.size();l++)
00472 {
00473 if (candidates[l]->id == laser_msg.Targets[i].id)
00474 {
00475 candidates[l]->distance = distance_to_target_from_center_bumper;
00476
00477 candidates[l]->world_upper_left = cv::Point3f(laser_msg.Targets[i].initialpose.x, laser_msg.Targets[i].initialpose.y, laser_msg.Targets[i].initialpose.z);
00478
00479 candidates[l]->world_down_right = cv::Point3f(laser_msg.Targets[i].finalpose.x, laser_msg.Targets[i].finalpose.y, laser_msg.Targets[i].finalpose.z);
00480
00481 candidates[l]->projectToImage(rvecs, tvecs, cameraMatrix,distCoeffs);
00482
00483 candidates[l]->marked_for_deletion = false;
00484
00485 id_found = true;
00486
00487
00488 break;
00489 }
00490 }
00491
00492 if (id_found == false)
00493 {
00494 Candidate::Ptr candidate = Candidate::Ptr(new Candidate(classifier_base));
00495
00496 candidate->id = laser_msg.Targets[i].id;
00497
00498
00499
00500 candidate->distance = distance_to_target_from_center_bumper;
00501
00502 candidate->world_upper_left = cv::Point3f(laser_msg.Targets[i].initialpose.x, laser_msg.Targets[i].initialpose.y, laser_msg.Targets[i].initialpose.z);
00503
00504 candidate->world_down_right = cv::Point3f(laser_msg.Targets[i].finalpose.x, laser_msg.Targets[i].finalpose.y, laser_msg.Targets[i].finalpose.z);
00505
00506 candidate->projectToImage(rvecs, tvecs, cameraMatrix,distCoeffs);
00507
00508 candidate->classification = UNKNOWN;
00509
00510 candidate->marked_for_deletion = false;
00511 candidate->classification_busy = false;
00512
00513 candidates.push_back(candidate);
00514 }
00515 }
00516 }
00517
00518 for(auto it = candidates.begin(); it!= candidates.end(); )
00519 {
00520 if((*it)->marked_for_deletion)
00521 {
00522 if((*it)->isBusy())
00523 (*it)->kill();
00524
00525 it = candidates.erase(it);
00526 }
00527 else
00528 it++;
00529 }
00530
00531
00532 sort(candidates.begin(), candidates.end(),[&](const Candidate::Ptr c1, const Candidate::Ptr c2)
00533 {
00534 return c1->distance < c2->distance;
00535 });
00536
00537 }
00538
00539 void initiallaserGather(const sensor_msgs::PointCloud2& laser_msg)
00540 {
00541
00542 double distance_to_target_from_center_bumper;
00543 double theta;
00544
00545 pcl::PCLPointCloud2 pcl_pc;
00546 pcl_conversions::toPCL(laser_msg, pcl_pc);
00547
00548 pcl::PointCloud<pcl::PointXYZ> cloud;
00549
00550 pcl::fromPCLPointCloud2(pcl_pc, cloud);
00551
00552 laser_points.clear();
00553
00554 for(int i=0; i<cloud.points.size();i++)
00555 {
00556 distance_to_target_from_center_bumper = sqrt(cloud.points[i].x * cloud.points[i].x +
00557 cloud.points[i].y * cloud.points[i].y);
00558
00559 theta = acos(cloud.points[i].x / distance_to_target_from_center_bumper);
00560
00561 if (theta < 0.5)
00562 {
00563 laser_points.push_back(cv::Point3f(cloud.points[i].x,cloud.points[i].y,cloud.points[i].z));
00564 }
00565 }
00566 }
00567
00568 };
00569
00570 int main(int argc, char** argv)
00571 {
00572 ros::init(argc, argv, "sensor_fusion");
00573
00574 ImageConverter ic;
00575
00576 ros::spin();
00577 return 0;
00578 }