00001 #include "camera_object_learner_alg.h"
00002
00003 CameraObjectLearnerAlgorithm::CameraObjectLearnerAlgorithm(void)
00004 {
00005 this->ptrPrms = &this->prms;
00006 this->ptrRFs = &this->RFs;
00007 this->ptrClfr = &this->clfr;
00008
00009 ptrRFs->fun_set_values(ptrPrms->fun_get_num_ferns(), ptrPrms->fun_get_num_features(), ptrPrms->fun_get_fern_size());
00010 ptrRFs->fun_random_ferns();
00011 }
00012
00013 CameraObjectLearnerAlgorithm::~CameraObjectLearnerAlgorithm(void)
00014 {
00015 }
00016
00017 void CameraObjectLearnerAlgorithm::config_update(Config& new_cfg, uint32_t level)
00018 {
00019 this->lock();
00020
00021
00022 this->config_=new_cfg;
00023
00024 this->unlock();
00025 }
00026
00027
00028 bool CameraObjectLearnerAlgorithm::init_detector(sensor_msgs::ImageConstPtr &image,cv::Rect &coord)
00029 {
00030 ROS_INFO("CameraObjectLearnerAlgorithm::init_detector");
00031 try
00032 {
00033 this->lock();
00034
00035 this->cv_image = cv_bridge::toCvCopy(image,sensor_msgs::image_encodings::BGR8);
00036 this->img=this->cv_image->image;
00037 this->show_img=this->cv_image->image;
00038
00039 CvRect box = cvRect(coord.x-coord.width/2,coord.y-coord.height/2 ,coord.width ,coord.height);
00040
00041 ROS_INFO("CameraObjectLearnerAlgorithm::init_detector: fun_train_classifier(..., img, box)");
00042 fun_train_classifier(this->ptrPrms, this->ptrClfr, this->ptrRFs, &this->img, box);
00043
00044 this->unlock();
00045 return true;
00046 }
00047 catch (cv_bridge::Exception& e)
00048 {
00049 this->unlock();
00050 ROS_ERROR("cv_bridge exception: %s", e.what());
00051 return false;
00052 }
00053 }
00054
00055 bool CameraObjectLearnerAlgorithm::detect(sensor_msgs::ImageConstPtr &image)
00056 {
00057 try
00058 {
00059 this->lock();
00060
00061 this->cv_image = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
00062 this->img=this->cv_image->image;
00063 this->show_img=this->cv_image->image;
00064
00065
00066
00067 this->detections = fun_detection(this->ptrPrms, this->ptrClfr, this->ptrRFs, &this->img);
00068
00069
00070 this->assistance = fun_check_assistance(this->ptrPrms, this->detections);
00071
00072
00073 if(this->assistance)
00074 {
00075
00076 fun_show_detections(this->ptrPrms, this->ptrClfr, &this->show_img, this->detections, this->assistance);
00077 this->unlock();
00078 return assistance;
00079 }
00080 else
00081 {
00082
00083 fun_learning(this->ptrPrms, this->ptrRFs, this->ptrClfr, &this->img, this->detections, this->assistance, true);
00084
00085
00086 fun_show_detections(this->ptrPrms, this->ptrClfr, &this->show_img, this->detections, this->assistance);
00087
00088 this->unlock();
00089 return this->assistance;
00090 }
00091 }
00092 catch (cv_bridge::Exception& e)
00093 {
00094 this->unlock();
00095 ROS_ERROR("cv_bridge exception: %s", e.what());
00096 return true;
00097 }
00098 }
00099
00100 bool CameraObjectLearnerAlgorithm::learn_detector(bool valid)
00101 {
00102 try
00103 {
00104 this->lock();
00105
00106
00107 fun_learning(this->ptrPrms, this->ptrRFs, this->ptrClfr, &this->img, this->detections, this->assistance, valid);
00108
00109
00110 fun_show_detections(this->ptrPrms, this->ptrClfr, &this->show_img, this->detections, this->assistance);
00111
00112 this->unlock();
00113 return true;
00114 }
00115 catch (cv_bridge::Exception& e)
00116 {
00117 this->unlock();
00118 ROS_ERROR("cv_bridge exception: %s", e.what());
00119 return false;
00120 }
00121 }
00122
00123 void CameraObjectLearnerAlgorithm::get_output_image(sensor_msgs::ImagePtr &image)
00124 {
00125 cv_bridge::CvImage roscv_outImage;
00126 roscv_outImage.header = this->cv_image->header;
00127 roscv_outImage.encoding = this->cv_image->encoding;
00128 roscv_outImage.image = cv::cvarrToMat(&this->show_img);
00129 image = roscv_outImage.toImageMsg();
00130 }