Fix for the leg_detector opencv3 bug

.travis.yml

@@ -9,10 +9,10 @@ env:
     - CATKIN_WS=~/catkin_ws
     - CATKIN_WS_SRC=${CATKIN_WS}/src
   matrix:
-    - CI_ROS_DISTRO="indigo"
+    - CI_ROS_DISTRO="kinetic"
     # - CI_ROS_DISTRO="jade"
 install:
-  - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+  - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
   - sudo apt-get update -qq
   - sudo apt-get install -qq -y python-rosdep python-catkin-tools

leg_detector/CMakeLists.txt

@@ -42,13 +42,13 @@ catkin_package(INCLUDE_DIRS include
   CATKIN_DEPENDS people_msgs sensor_msgs std_msgs geometry_msgs visualization_msgs)
 
 ## Declare a cpp executable
-add_executable(leg_detector 
+add_executable(leg_detector
                src/laser_processor.cpp
-               src/leg_detector.cpp 
+               src/leg_detector.cpp
                src/calc_leg_features.cpp)
 
 ## Add cmake target dependencies of the executable/library
-add_dependencies(leg_detector people_msgs_gencpp ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(leg_detector ${${PROJECT_NAME}_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(leg_detector

leg_detector/src/leg_detector.cpp

@@ -244,7 +244,8 @@ class LegDetector
 
   int mask_count_;
 
-  cv::ml::RTrees forest;
+  // cv::ml::RTrees forest;
+  cv::Ptr<cv::ml::RTrees> forest;
 
   float connected_thresh_;
 
@@ -286,8 +287,10 @@ class LegDetector
   {
     if (g_argc > 1)
     {
-      forest.load(g_argv[1]);
-      feat_count_ = forest.get_active_var_mask()->cols;
+      forest = cv::ml::RTrees::create();
+      cv::String feature_file = cv::String(g_argv[1]);
+      forest = cv::ml::StatModel::load<cv::ml::RTrees>(feature_file);
+      feat_count_ = forest->getVarCount();
       printf("Loaded forest with %d features: %s\n", feat_count_, g_argv[1]);
     }
     else
@@ -684,7 +687,7 @@ class LegDetector
     processor.splitConnected(connected_thresh_);
     processor.removeLessThan(5);
 
-    CvMat* tmp_mat = cvCreateMat(1, feat_count_, CV_32FC1);
+    cv::Mat tmp_mat = cv::Mat(1, feat_count_, CV_32FC1);
 
     // if no measurement matches to a tracker in the last <no_observation_timeout>  seconds: erase tracker
     ros::Time purge = scan->header.stamp + ros::Duration().fromSec(-no_observation_timeout_s);
@@ -725,9 +728,14 @@ class LegDetector
       vector<float> f = calcLegFeatures(*i, *scan);
 
       for (int k = 0; k < feat_count_; k++)
-        tmp_mat->data.fl[k] = (float)(f[k]);
-
-      float probability = forest.predict_prob(tmp_mat);
+        tmp_mat.data[k] = (float)(f[k]);
+
+      // Probability is the fuzzy measure of the probability that the second element should be chosen,
+      // in opencv2 RTrees had a method predict_prob, but that disapeared in opencv3, this is the
+      // substitute.
+      float probability = 0.5 -
+                          forest->predict(tmp_mat, cv::noArray(), cv::ml::RTrees::PREDICT_SUM) /
+                          forest->getRoots().size();
       Stamped<Point> loc((*i)->center(), scan->header.stamp, scan->header.frame_id);
       try
       {
@@ -841,8 +849,6 @@ class LegDetector
       }
     }
 
-    cvReleaseMat(&tmp_mat);
-    tmp_mat = 0;
     if (!use_seeds_)
       pairLegs();
 

people_tracking_filter/CMakeLists.txt

@@ -39,26 +39,26 @@ include_directories(
 )
 
 ## Declare a cpp library
-add_library(people_tracking_filter 
-            src/uniform_vector.cpp 
-            src/gaussian_vector.cpp 
-            src/gaussian_pos_vel.cpp 
+add_library(people_tracking_filter
+            src/uniform_vector.cpp
+            src/gaussian_vector.cpp
+            src/gaussian_pos_vel.cpp
             src/mcpdf_pos_vel.cpp
             src/mcpdf_vector.cpp
             src/sysmodel_pos_vel.cpp
             src/sysmodel_vector.cpp
             src/measmodel_pos.cpp
             src/measmodel_vector.cpp
-	    src/tracker_particle.cpp 
-	    src/tracker_kalman.cpp 
-	    src/detector_particle.cpp 
+	    src/tracker_particle.cpp
+	    src/tracker_kalman.cpp
+	    src/detector_particle.cpp
 )
 
 ## Declare a cpp executable
 add_executable(people_tracker src/people_tracking_node.cpp)
 
 ## Add cmake target dependencies of the executable/library
-add_dependencies(people_tracker people_msgs_gencpp people_tracking_filter)
+add_dependencies(people_tracker people_tracking_filter)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(people_tracker