Go to the documentation of this file.00001
00060
00061
00062
00063 #include <munkres/munkres.h>
00064
00065
00066
00067
00068
00069 #include <ros/ros.h>
00070 #include <ros/package.h>
00071
00072
00073 #include <sensor_msgs/Image.h>
00074
00075 #include <cob_perception_msgs/DetectionArray.h>
00076
00077
00078
00079
00080
00081 #include <message_filters/subscriber.h>
00082 #include <message_filters/synchronizer.h>
00083 #include <message_filters/sync_policies/approximate_time.h>
00084 #include <image_transport/image_transport.h>
00085 #include <image_transport/subscriber_filter.h>
00086
00087
00088 #include <opencv2/opencv.hpp>
00089 #include <cv_bridge/cv_bridge.h>
00090 #include <sensor_msgs/image_encodings.h>
00091
00092
00093 #include <boost/bind.hpp>
00094 #include <boost/thread/mutex.hpp>
00095 #include <boost/lexical_cast.hpp>
00096
00097
00098 #include "cob_vision_utils/GlobalDefines.h"
00099
00100 #include <sstream>
00101 #include <string>
00102 #include <vector>
00103
00104 class EvalObj
00105 {
00106 public:
00107 ros::Time timestamp;
00108 std::vector<geometry_msgs::Point> positions;
00109 std::vector<std::string> labels;
00110 EvalObj()
00111 {
00112 }
00113 void print()
00114 {
00115 std::cout << "-----------------------------" << std::endl;
00116 std::cout << "Timestamp:\n" << timestamp << std::endl;
00117 std::cout << "Positions:" << std::endl;
00118 for (int i = 0; i < positions.size(); i++)
00119 {
00120 std::cout << positions[i].x << " " << positions[i].y << std::endl;
00121 ;
00122 }
00123 std::cout << "Labels:" << std::endl;
00124 for (int i = 0; i < labels.size(); i++)
00125 {
00126 std::cout << labels[i] << std::endl;
00127 ;
00128 }
00129 std::cout << "-----------------------------" << std::endl;
00130 }
00131
00132 bool compare(EvalObj& cmp, std::string& gt, bool& this_corr, bool& cmp_corr)
00133 {
00134 if (cmp.timestamp != this->timestamp)
00135 {
00136 std::cout << "time discrepancy - skipping comparison" << std::endl;
00137 return false;
00138 }
00139 else
00140 {
00141
00142 float dist = 100000;
00143 std::vector<int> index_arr;
00144 index_arr.resize(this->labels.size());
00145 if (this->labels.size() == 0)
00146 {
00147 std::cout << "empty label array...." << std::endl;
00148 return false;
00149 }
00150 if (cmp.labels.size() == 0)
00151 {
00152 std::cout << "empty label array...." << std::endl;
00153 return false;
00154 }
00155 for (int i = 0; i < this->labels.size(); i++)
00156 {
00157 for (int j = 0; j < cmp.labels.size(); j++)
00158 {
00159
00160 float tmp_dist = (cmp.positions[i].x - this->positions[j].x) * (cmp.positions[i].x - this->positions[j].y) + (cmp.positions[i].y - this->positions[j].y)
00161 * (cmp.positions[i].y - this->positions[j].y);
00162 if (tmp_dist < dist)
00163 {
00164 dist = tmp_dist;
00165 index_arr[i] = j;
00166 }
00167 }
00168 }
00169
00170
00171
00172
00173
00174
00175 this_corr = true;
00176 cmp_corr = true;
00177 for (int i; i < index_arr.size(); i++)
00178 {
00179 std::string a = boost::lexical_cast<std::string>(this->labels[i]);
00180 std::string b = boost::lexical_cast<std::string>(cmp.labels[index_arr[i]]);
00181 std::cout << a << ": " << gt.compare(a) << " - " << b << ": " << gt.compare(b) << std::endl;
00182 if (a.compare("UnknownHead") == 0 || b.compare("UnknownHead") == 0)
00183 {
00184 std::cout << "discarding" << std::endl;
00185 return false;
00186 }
00187
00188 if (a.compare(gt) != 0)
00189 this_corr = false;
00190 if (b.compare(gt) != 0)
00191 cmp_corr = false;
00192
00193 }
00194
00195 }
00196 return true;
00197 }
00198 };
00199
00200 class TrackingEvalNode
00201 {
00202 public:
00203
00204 EvalObj eval_rec_;
00205 EvalObj eval_track_;
00206 bool rec_track;
00207 bool rec_rec;
00208 std::string gt_name_;
00209
00210 int correct_trackings_;
00211 int correct_recognitions_;
00212 int total_comparisons_;
00213 int tracking_improvement_;
00214 int tracking_decline_;
00215 int error_out_;
00216
00217 ros::Subscriber recognition_subscriber_;
00218 ros::Subscriber tracking_subscriber_;
00219 ros::NodeHandle node_handle_;
00220
00221 TrackingEvalNode(ros::NodeHandle nh) :
00222 node_handle_(nh), correct_trackings_(0), correct_recognitions_(0), total_comparisons_(0), error_out_(0), tracking_improvement_(0), tracking_decline_(0), rec_track(false),
00223 rec_rec(false)
00224 {
00225
00226
00227 recognition_subscriber_ = node_handle_.subscribe("/cob_people_detection/face_recognizer/face_recognitions", 1, &TrackingEvalNode::recognitionCallback, this);
00228 tracking_subscriber_ = node_handle_.subscribe("/cob_people_detection/detection_tracker/face_position_array", 1, &TrackingEvalNode::trackingCallback, this);
00229
00230
00231 if (!node_handle_.getParam("/cob_people_detection/tracking_evaluator/person", gt_name_))
00232 std::cout << "PARAM NOT AVAILABLE" << std::endl;
00233 std::cout << "Groundtruth name: " << gt_name_ << "\n";
00234
00235 }
00236
00237 ~TrackingEvalNode()
00238 {
00239 }
00240
00241 double computeFacePositionDistance(const cob_perception_msgs::Detection& previous_detection, const cob_perception_msgs::Detection& current_detection)
00242 {
00243 const geometry_msgs::Point* point_1 = &(previous_detection.pose.pose.position);
00244 const geometry_msgs::Point* point_2 = &(current_detection.pose.pose.position);
00245
00246 double dx = point_1->x - point_2->x;
00247 double dy = point_1->y - point_2->y;
00248 double dz = point_1->z - point_2->z;
00249
00250 return sqrt(dx * dx + dy * dy + dz * dz);
00251 }
00252
00254 void trackingCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_position_msg_in)
00255 {
00256
00257
00258 EvalObj eval_obj;
00259 eval_obj.timestamp = face_position_msg_in->header.stamp;
00260
00261
00262 for (unsigned int i = 0; i < face_position_msg_in->detections.size(); i++)
00263 {
00264 if (face_position_msg_in->detections[i].label.compare("") != 0)
00265 {
00266
00267 eval_obj.positions.push_back(face_position_msg_in->detections[i].pose.pose.position);
00268 eval_obj.labels.push_back(face_position_msg_in->detections[i].label);
00269 }
00270 }
00271 eval_track_ = eval_obj;
00272 rec_track = true;
00273 this->eval();
00274 }
00276 void recognitionCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_position_msg_in)
00277 {
00278
00279
00280 EvalObj eval_obj;
00281 eval_obj.timestamp = face_position_msg_in->header.stamp;
00282
00283
00284 for (unsigned int i = 0; i < face_position_msg_in->detections.size(); i++)
00285 {
00286 if (face_position_msg_in->detections[i].label.compare("") != 0)
00287 {
00288
00289 eval_obj.positions.push_back(face_position_msg_in->detections[i].pose.pose.position);
00290 eval_obj.labels.push_back(face_position_msg_in->detections[i].label);
00291 }
00292 }
00293 eval_rec_ = eval_obj;
00294 rec_rec = true;
00295 this->eval();
00296 }
00297
00298 void eval()
00299 {
00300 if (rec_track == true && rec_rec == true)
00301 {
00302
00303
00304 if (eval_track_.timestamp == eval_rec_.timestamp)
00305 {
00306
00307 bool rec_res, track_res;
00308 bool cmp_successful = eval_rec_.compare(eval_track_, gt_name_, rec_res, track_res);
00309 if (cmp_successful)
00310 {
00311 if (rec_res)
00312 correct_recognitions_++;
00313 if (track_res)
00314 correct_trackings_++;
00315 if (track_res && !rec_res)
00316 tracking_improvement_++;
00317 if (!track_res && rec_res)
00318 tracking_decline_++;
00319 if (!track_res && !rec_res)
00320 error_out_++;
00321 total_comparisons_++;
00322 this->print_result();
00323 }
00324 rec_rec = false;
00325 rec_rec = false;
00326 }
00327 }
00328 }
00329 void print_result()
00330 {
00331 std::cout << "-----------------------------------" << std::endl;
00332 std::cout << "total comparisons : " << total_comparisons_ << std::endl;
00333 std::cout << "correctly recognized : " << ((float)correct_recognitions_ / (float)total_comparisons_) * 100 << " \%" << std::endl;
00334 std::cout << "correctly tracked : " << ((float)correct_trackings_ / (float)total_comparisons_) * 100 << " \%" << std::endl;
00335 std::cout << "tracking improvement : " << ((float)tracking_improvement_ / (float)total_comparisons_) * 100 << " \%" << std::endl;
00336 std::cout << "tracking worsening : " << ((float)tracking_decline_ / (float)total_comparisons_) * 100 << " \%" << std::endl;
00337 std::cout << "errors after tracking: " << (error_out_ / total_comparisons_) * 100 << " \%" << std::endl;
00338 std::cout << "-----------------------------------" << std::endl;
00339
00340
00341
00342
00343
00344
00345
00346
00347 }
00348 };
00349
00350
00351
00352 int main(int argc, char** argv)
00353 {
00354
00355 ros::init(argc, argv, "tracking_evaluator");
00356
00357
00358 ros::NodeHandle nh;
00359
00360
00361 TrackingEvalNode tracking_eval_node(nh);
00362
00363
00364
00365
00366
00367
00368 ros::spin();
00369
00370 return 0;
00371 }