[Fawkes Git] branch/vmatare/laser-lines-matching-refactored: created (0.5.0-3215-g22c0ea8)

Victor Mataré matare at lih.rwth-aachen.de
Wed Aug 31 05:19:45 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, vmatare/laser-lines-matching-refactored has been created
        at  22c0ea8a615c7d0a5d6994df5f115bf1f8610e1e (commit)

http://git.fawkesrobotics.org/fawkes.git/vmatare/laser-lines-matching-refactored

- *Log* ---------------------------------------------------------------
commit 22c0ea8a615c7d0a5d6994df5f115bf1f8610e1e
Author:     Victor Mataré <matare at lih.rwth-aachen.de>
AuthorDate: Sat Jun 25 19:59:27 2016 +0200
Commit:     Victor Mataré <matare at lih.rwth-aachen.de>
CommitDate: Wed Aug 31 05:19:31 2016 +0200

    laser-lines: match lines combinatorially by distance

http://git.fawkesrobotics.org/fawkes.git/commit/22c0ea8
http://trac.fawkesrobotics.org/changeset/22c0ea8

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------


- *Diffs* -------------------------------------------------------------

- *commit* 22c0ea8a615c7d0a5d6994df5f115bf1f8610e1e - - - - - - - - - -
Author:  Victor Mataré <matare at lih.rwth-aachen.de>
Date:    Sat Jun 25 19:59:27 2016 +0200
Subject: laser-lines: match lines combinatorially by distance

 src/plugins/laser-lines/Makefile               |    2 +-
 src/plugins/laser-lines/laser-lines-thread.cpp |  153 +++++++++++-------------
 src/plugins/laser-lines/laser-lines-thread.h   |    4 +-
 src/plugins/laser-lines/line_info.cpp          |  111 +++++++++++++++++
 src/plugins/laser-lines/line_info.h            |   26 ++++
 5 files changed, 209 insertions(+), 87 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/Makefile b/src/plugins/laser-lines/Makefile
index 89e85ac..253a730 100644
--- a/src/plugins/laser-lines/Makefile
+++ b/src/plugins/laser-lines/Makefile
@@ -39,7 +39,7 @@ CFLAGS += $(CFLAGS_CPP11) -Wno-unknown-pragmas
 LIBS_laser_lines = fawkescore fawkesutils fawkesaspects fvutils fawkesbaseapp \
 			fawkestf fawkesinterface fawkesblackboard fawkespcl_utils \
 			Position3DInterface SwitchInterface LaserLineInterface
-OBJS_laser_lines = laser-lines-plugin.o laser-lines-thread.o
+OBJS_laser_lines = laser-lines-plugin.o laser-lines-thread.o  line_info.o
 
 OBJS_all    = $(OBJS_laser_lines)
 
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index 787b75a..b9faf65 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -117,7 +117,6 @@ LaserLinesThread::init()
     if(cfg_moving_avg_enabled_)
     {
       line_avg_ifs_.resize(cfg_max_num_lines_, NULL);
-      moving_average_windows_.resize(cfg_max_num_lines_);
     }
     for (unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
       char *tmp;
@@ -270,6 +269,7 @@ LaserLinesThread::loop()
   //logger->log_info(name(), "[L %u] total: %zu   finite: %zu",
   //		     loop_count_, input_->points.size(), in_cloud->points.size());
 
+  {
   std::vector<LineInfo> linfos =
     calc_lines<PointType>(input_,
 			  cfg_segm_min_inliers_, cfg_segm_max_iterations_,
@@ -289,96 +289,77 @@ LaserLinesThread::loop()
   lines_->height = 1;
   lines_->width  = num_points;
 
-  // sort lines by bearing to stabilize IDs
-  std::sort(linfos.begin(), linfos.end(),
-	    [](const LineInfo &l1, const LineInfo &l2) -> bool
-	    {
-	      return l1.bearing < l2.bearing;
-	    });
-
-  if (linfos.size() > cfg_max_num_lines_){
-    // Ignore lines if more than cfg_max_num_lines_ found.
-    // Since the lines are orderd by bearing, this will drop
-    // the line with the highest bearing.
-    linfos.resize(cfg_max_num_lines_);
+  vector<TrackedLineInfo>::iterator known_it = known_lines_.begin();
+  while (known_it != known_lines_.end()) {
+    btScalar min_dist = numeric_limits<btScalar>::max();
+    auto best_match = linfos.end();
+    for (vector<LineInfo>::iterator it_new = linfos.begin(); it_new != linfos.end(); ++it_new) {
+      btScalar d = known_it->distance(*it_new);
+      if (d < min_dist) {
+	min_dist = d;
+	best_match = it_new;
+      }
+    }
+    if (best_match != linfos.end() && min_dist < cfg_switch_tolerance_) {
+      known_it->update(*best_match);
+
+      // Important: erase line because all lines remaining after this are considered "new" (see below)
+      linfos.erase(best_match);
+      ++known_it;
+    }
+    else // No match for this line, so kill it
+      known_it = known_lines_.erase(known_it);
   }
 
+  for (LineInfo &l : linfos) {
+    // Only unmatched lines remaining, so these are the "new" lines
+    TrackedLineInfo tl(
+	tf_listener,
+	finput_->header.frame_id,
+	cfg_switch_tolerance_,
+	cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1);
+    tl.update(l);
+    known_lines_.push_back(tl);
+  }
 
-  std::vector<LineInfo> linfos_filtered;
-  linfos_filtered.resize(linfos.size());
-  if(cfg_moving_avg_enabled_)
-  {
-    
-    for(size_t info_cntr = 0; info_cntr < linfos.size(); ++info_cntr)
-    {
-      std::deque<LineInfo> &moving_avg_window = moving_average_windows_[info_cntr];
-      LineInfo &info = linfos[info_cntr];
-      LineInfo &filtered = linfos_filtered[info_cntr];
-      moving_avg_window.push_front(LineInfo(info));
-      if(moving_avg_window.size() > cfg_moving_avg_window_size_) {
-        moving_avg_window.pop_back();
+  }
+
+  // When there are too many lines, delete the ones farthest away
+  std::sort(known_lines_.begin(), known_lines_.end(),
+      [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool
+      {
+	return l1.raw.point_on_line.norm() < l2.raw.point_on_line.norm();
       }
-      
-      float if_point_on_line_avg[3] = {0,0,0};
-      float if_line_direction_avg[3] = {0,0,0};
-      float if_end_point_1_avg[3] = {0,0,0};
-      float if_end_point_2_avg[3] = {0,0,0};
-      float bearing_avg = 0;
-      float length_avg = 0;
-      
-      for(const LineInfo &avg_info: moving_avg_window) {
-        if_point_on_line_avg[0] += avg_info.base_point[0];
-        if_point_on_line_avg[1] += avg_info.base_point[1];
-        if_point_on_line_avg[2] += avg_info.base_point[2];
-        if_line_direction_avg[0] += avg_info.line_direction[0];
-        if_line_direction_avg[1] += avg_info.line_direction[1];
-        if_line_direction_avg[2] += avg_info.line_direction[2];
-        if_end_point_1_avg[0] += avg_info.end_point_1[0];
-        if_end_point_1_avg[1] += avg_info.end_point_1[1];
-        if_end_point_1_avg[2] += avg_info.end_point_1[2];
-        if_end_point_2_avg[0] += avg_info.end_point_2[0];
-        if_end_point_2_avg[1] += avg_info.end_point_2[1];
-        if_end_point_2_avg[2] += avg_info.end_point_2[2];
-        bearing_avg += avg_info.bearing;
-        length_avg += avg_info.length;
+  );
+  while (known_lines_.size() > cfg_max_num_lines_)
+    known_lines_.erase(known_lines_.end() - 1);
+
+  // Then sort by bearing to stabilize blackboard interface assignment
+  std::sort(known_lines_.begin(), known_lines_.end(),
+      [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool
+      {
+	return l1.bearing_center < l2.bearing_center;
       }
-      
-      size_t queue_size = moving_avg_window.size();
-      filtered.base_point = {if_point_on_line_avg[0] / queue_size,
-                             if_point_on_line_avg[1] / queue_size,
-                             if_point_on_line_avg[2] / queue_size};
-      filtered.line_direction = {if_line_direction_avg[0] / queue_size,
-                                 if_line_direction_avg[1] / queue_size,
-                                 if_line_direction_avg[2] / queue_size};
-      filtered.end_point_1 = {if_end_point_1_avg[0] / queue_size,
-                              if_end_point_1_avg[1] / queue_size,
-                              if_end_point_1_avg[2] / queue_size};
-      filtered.end_point_2 = {if_end_point_2_avg[0] / queue_size,
-                              if_end_point_2_avg[1] / queue_size,
-                              if_end_point_2_avg[2] / queue_size};
-      filtered.bearing = bearing_avg / queue_size;
-      filtered.length = length_avg / queue_size;
-    }
-  }
+  );
+
   // set line parameters
   size_t oi = 0;
   unsigned int line_if_idx = 0;
-  for (size_t i = 0; i < linfos.size(); ++i) {
-    const LineInfo &info = linfos[i];
-    const LineInfo &info_avg = linfos_filtered[i];
+  for (size_t i = 0; i < known_lines_.size(); ++i) {
+    const TrackedLineInfo &info = known_lines_[i];
 
     if (line_if_idx < cfg_max_num_lines_) {
-	    set_line(line_if_idx, line_ifs_[line_if_idx], true, finput_->header.frame_id, info);
+      set_line(line_if_idx, line_ifs_[line_if_idx], true, finput_->header.frame_id, info.raw);
       if(cfg_moving_avg_enabled_)
       {
-        set_line(line_if_idx, line_avg_ifs_[line_if_idx], true, finput_->header.frame_id, info_avg);
+        set_line(line_if_idx, line_avg_ifs_[line_if_idx], true, finput_->header.frame_id, info.smooth);
       }
       line_if_idx++;
     }
 
-    for (size_t p = 0; p < info.cloud->points.size(); ++p) {
+    for (size_t p = 0; p < info.raw.cloud->points.size(); ++p) {
       ColorPointType &out_point = lines_->points[oi++];
-      PointType &in_point  = info.cloud->points[p];
+      PointType &in_point  = info.raw.cloud->points[p];
       out_point.x = in_point.x;
       out_point.y = in_point.y;
       out_point.z = in_point.z;
@@ -394,17 +375,23 @@ LaserLinesThread::loop()
   }
 
   for (unsigned int i = line_if_idx; i < cfg_max_num_lines_; ++i) {
-	  set_line(i, line_ifs_[i], false);
+    set_line(i, line_ifs_[i], false);
     if(cfg_moving_avg_enabled_)
     {
-	    set_line(i, line_avg_ifs_[i], false);
+      set_line(i, line_avg_ifs_[i], false);
     }
   }
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  publish_visualization(linfos, "laser_lines");
-  if(cfg_moving_avg_enabled_) {
-	  publish_visualization(linfos_filtered, "laser_lines_moving_average", "_avg");
+  { unsigned int line_id = 0;
+  for (size_t i = 0; i < known_lines_.size(); i++) {
+    publish_visualization(known_lines_[i].raw, i, line_id, "laser_lines");
+    line_id++;
+    if (cfg_moving_avg_enabled_) {
+      publish_visualization(known_lines_[i].smooth, i, line_id, "laser_lines_moving_average", "_avg");
+      line_id++;
+    }
+  }
   }
 #endif
 
@@ -429,6 +416,7 @@ LaserLinesThread::loop()
 }
 
 
+
 void
 LaserLinesThread::set_line(unsigned int idx,
                            fawkes::LaserLineInterface *iface,
@@ -523,15 +511,12 @@ LaserLinesThread::set_line(unsigned int idx,
 
 #ifdef HAVE_VISUAL_DEBUGGING
 void
-LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos,
+LaserLinesThread::publish_visualization(const LineInfo &info, size_t i, unsigned int idnum,
                                         std::string marker_namespace, std::string name_suffix)
 {
   visualization_msgs::MarkerArray m;
-  unsigned int idnum = 0;
 
-  for (size_t i = 0; i < linfos.size(); ++i) {
-    const LineInfo &info = linfos[i];
-    
+  {
     /*
     visualization_msgs::Marker basevec;
     basevec.header.frame_id = finput_->header.frame_id;
diff --git a/src/plugins/laser-lines/laser-lines-thread.h b/src/plugins/laser-lines/laser-lines-thread.h
index 136cf13..a531a44 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -105,7 +105,7 @@ class LaserLinesThread
 
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  void publish_visualization(const std::vector<LineInfo> &linfos,
+  void publish_visualization(const LineInfo &linfo, size_t i, unsigned int idnum,
                              std::string marker_namespace, std::string name_suffix = "");
 #endif
 
@@ -117,7 +117,7 @@ class LaserLinesThread
 
   std::vector<fawkes::LaserLineInterface *> line_ifs_;
   std::vector<fawkes::LaserLineInterface *> line_avg_ifs_;
-  std::vector<std::deque<LineInfo>> moving_average_windows_;
+  std::vector<TrackedLineInfo> known_lines_;
 
   fawkes::SwitchInterface *switch_if_;
 
diff --git a/src/plugins/laser-lines/line_info.h b/src/plugins/laser-lines/line_info.h
index 9c60765..e77c154 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -25,6 +25,10 @@
 #include <Eigen/Geometry>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <boost/circular_buffer.hpp>
+#include <memory>
+#include <tf/types.h>
+#include <tf/transformer.h>
 
 /** Line information container.
  * All points and angles are in the sensor reference frame
@@ -49,4 +53,26 @@ class LineInfo {
   						///< points account to this line
 };
 
+
+class TrackedLineInfo {
+public:
+  LineInfo raw;   	///< the latest geometry of this line, i.e. unfiltered
+  LineInfo smooth;	///< moving-average geometry of this line (cf. length of history buffer)
+  fawkes::tf::Stamped<fawkes::tf::Point> base_point_odom;	///< last reference point (in odom frame) for line tracking
+  fawkes::tf::Transformer *transformer;	///< Transformer used to transform from input_frame_id_to odom
+  std::string input_frame_id;	///< Input frame ID of raw line infos (base_laser usually)
+  float cfg_switch_tolerance;	///< Configured line jitter threshold
+  boost::circular_buffer<LineInfo> history;	///< history of raw line geometries for computing moving average
+  float bearing_center; 	///< Bearing towards line center, used to select lines "in front of us" when there
+
+  TrackedLineInfo(
+      fawkes::tf::Transformer *tfer,
+      const std::string &input_frame_id,
+      float cfg_switch_tolerance,
+      unsigned int cfg_moving_avg_len);
+
+  btScalar distance(const LineInfo &linfo) const;
+  void update(LineInfo &new_linfo);
+};
+
 #endif




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list