sensor_fusion.cpp
Go to the documentation of this file.
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> //I believe you were using pcl/ros/conversion.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 // static const char WINDOW[] = "Image window";
00045 // static const char WINDOW1[] = "Image window1";
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 //         Mat image;
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             // Project Laser Points in Image
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));// distance*100*(1-cos((3.14/2)/ distance))
00165             image_down_left = Point2f(image_points[1].x - 50, image_points[1].y + 1050*tan(1./distance));// image_points[3];
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             //boundary conditions
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(); // inicia a contagem do tempo
00239             
00240             PedestrianClassifier classifier(classifier_base);
00241             
00242             //Run the heady classification code from pedro
00243             classification = classifier.classify(object_image);
00244             
00245 //                 if(classification == PEDESTRIAN)
00246 //                     cout<<"Detetei um peao " << id  <<endl;
00247 //                 else
00248 //                     cout<<"Nao detetei um peao " << id <<endl;
00249             
00250 //                 cout << "duracao: " << ros::Time::now().toSec() -secs << endl; // fornece o tempo total que o programa demora a correr a imagem
00251             
00252 //             cout << "out of classification" << endl;
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();//cout<<"waiting for slaughter: "<<id<<endl;
00271             object_thread.join();//cout<<"thread is dead: "<<id<<endl;
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     //         image_sub_ = it_.subscribe("image_raw", 1, &ImageConverter::imageCb, this);
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 //             nh_.param<string>("classifier",classifier_file,string("not_found"));
00329 //             cout << "classifier_file" << classifier_file << endl;
00330             
00331             classifier_file = "/home/asus/workingcopies/lar4/src/perception/pedestrians/multimodal_pedestrian_detect/trained_boost_15Kf_2000w_19Ks_m8_M64_boot3.xml" ;
00332 //             cout << "classifier_file1" << classifier_file << endl;
00333     //         cv::namedWindow(WINDOW);
00334     //         cv::namedWindow(WINDOW1);
00335             
00336             classifier_base->load(classifier_file);
00337             
00338             number_of_threads = 0;
00339             
00340         }
00341 
00342         ~ImageConverter()
00343         {
00344     //         cv::destroyWindow(WINDOW);
00345     //         cv::destroyWindow(WINDOW1);
00346         }
00347         
00348         void cameraInfoCallback(const sensor_msgs::CameraInfo& cam_info_msg)
00349         {
00350             // Read Extrinsic Camera Parameters (D, R ,P) to extract cameraMatrix, distCoeffs, rvecs, tvecs
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(); // the subscriber only runs one time
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             // Project Laser Points in Image
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             // Classificação dos objectos
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 //             cout << "received laser: " << laser_msg.Targets.size() << endl;
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 /*&& laser_msg.Targets[i].pose.position.y > -1.0 && laser_msg.Targets[i].pose.position.y < 2.15*/)
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 //                             cout << "candidate: " << candidates[l]->classification << endl;
00487 //                             cout << "updated candidate: " <<candidates[l]->id<< endl;
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 //                         cout << "new candidate: " << candidate->id << endl;
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 //                     cout << "removing candidate: " << (*it)->id << endl;
00525                     it = candidates.erase(it); 
00526                 }
00527                 else 
00528                     it++;
00529             }
00530             
00531             // Ordenar os candidatos pela distancia a que estao do Laser
00532             sort(candidates.begin(), candidates.end(),[&](const Candidate::Ptr c1, const Candidate::Ptr c2)
00533             { 
00534                 return c1->distance < c2->distance; 
00535             });
00536 //             cout << "processed laser" << endl << endl;
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     //         cout << "i'm here" << endl;
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 }


multimodal_pedestrian_detect
Author(s): Rui Azevedo
autogenerated on Fri May 30 2014 16:11:19