[Fawkes Git] branch/vmatare/laser-lines-matching: laser-lines: refactor tracking & moving-avg

Victor Mataré matare at lih.rwth-aachen.de
Wed Aug 31 05:07:08 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 has been updated
        to  db950ed11756d6f2b4141a1af6c062f2366d0df9 (commit)
      from  992b0e083eccf8264609894c612238a81adebd8d (commit)

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

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit db950ed11756d6f2b4141a1af6c062f2366d0df9
Author:     Victor Mataré <matare at lih.rwth-aachen.de>
AuthorDate: Wed Aug 31 05:04:45 2016 +0200
Commit:     Victor Mataré <matare at lih.rwth-aachen.de>
CommitDate: Wed Aug 31 05:06:49 2016 +0200

    laser-lines: refactor tracking & moving-avg

http://git.fawkesrobotics.org/fawkes.git/commit/db950ed
http://trac.fawkesrobotics.org/changeset/db950ed

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


- *Summary* -----------------------------------------------------------
 src/plugins/laser-lines/Makefile               |    2 +-
 src/plugins/laser-lines/laser-lines-thread.cpp |  145 ++++++------------------
 src/plugins/laser-lines/line_info.cpp          |  111 ++++++++++++++++++
 src/plugins/laser-lines/line_info.h            |   23 +++-
 4 files changed, 167 insertions(+), 114 deletions(-)
 create mode 100644 src/plugins/laser-lines/line_info.cpp


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

- *commit* db950ed11756d6f2b4141a1af6c062f2366d0df9 - - - - - - - - - -
Author:  Victor Mataré <matare at lih.rwth-aachen.de>
Date:    Wed Aug 31 05:04:45 2016 +0200
Subject: laser-lines: refactor tracking & moving-avg

 src/plugins/laser-lines/Makefile               |    2 +-
 src/plugins/laser-lines/laser-lines-thread.cpp |  145 ++++++------------------
 src/plugins/laser-lines/line_info.cpp          |  111 ++++++++++++++++++
 src/plugins/laser-lines/line_info.h            |   23 +++-
 4 files changed, 167 insertions(+), 114 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 eead7d6..9444605 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -291,66 +291,19 @@ LaserLinesThread::loop()
 
   vector<TrackedLineInfo>::iterator known_it = known_lines_.begin();
   while (known_it != known_lines_.end()) {
-    btScalar min_dist = std::numeric_limits<btScalar>::max();
-    fawkes::tf::Stamped<fawkes::tf::Point> best_new_bp_odom;
-    vector<LineInfo>::iterator best_match = linfos.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) {
-      fawkes::tf::Stamped<fawkes::tf::Point> bp_new(
-	  fawkes::tf::Point(
-	      it_new->base_point[0], it_new->base_point[1], it_new->base_point[2]
-	  ), fawkes::Time(0,0), finput_->header.frame_id);
-      fawkes::tf::Stamped<fawkes::tf::Point> bp_odom_new;
-      tf_listener->transform_point("/odom", bp_new, bp_odom_new);
-
-      fawkes::tf::Point d = bp_odom_new - known_it->base_point_odom;
-
-      if (d.length2() < min_dist) {
-	min_dist = d.length2();
+      btScalar d = known_it->distance(*it_new);
+      if (d < min_dist) {
+	min_dist = d;
 	best_match = it_new;
-	best_new_bp_odom = bp_odom_new;
       }
     }
     if (best_match != linfos.end() && min_dist < cfg_switch_tolerance_) {
-      known_it->base_point = best_match->base_point;
-      known_it->bearing = best_match->bearing;
-      known_it->cloud = best_match->cloud;
-      known_it->end_point_1 = best_match->end_point_1;
-      known_it->end_point_2 = best_match->end_point_2;
-      known_it->length = best_match->length;
-      known_it->line_direction = best_match->line_direction;
-      known_it->point_on_line = best_match->point_on_line;
-      known_it->base_point_odom = best_new_bp_odom;
-      known_it->history.push_back(*best_match);
-
-      Eigen::Vector3f base_point_sum(0,0,0), end_point_1_sum(0,0,0),
-	  end_point_2_sum(0,0,0), line_direction_sum(0,0,0), point_on_line_sum(0,0,0);
-      float length_sum(0);
-      for (LineInfo &l : known_it->history) {
-	base_point_sum += l.base_point;
-	end_point_1_sum += l.end_point_1;
-	end_point_2_sum += l.end_point_2;
-	line_direction_sum += l.line_direction;
-	point_on_line_sum += l.point_on_line;
-	length_sum += l.length;
-      }
-
-      size_t sz = known_it->history.size();
-      known_it->smooth.base_point = base_point_sum / sz;
-      known_it->smooth.cloud = best_match->cloud;
-      known_it->smooth.end_point_1 = end_point_1_sum / sz;
-      known_it->smooth.end_point_2 = end_point_2_sum / sz;
-      known_it->smooth.length = length_sum / sz;
-      known_it->smooth.line_direction = line_direction_sum / sz;
-      known_it->smooth.point_on_line = point_on_line_sum / sz;
-
-      Eigen::Vector3f ld_unit = known_it->smooth.line_direction / known_it->smooth.line_direction.norm();
-      Eigen::Vector3f pol_invert = Eigen::Vector3f(0,0,0) - known_it->smooth.point_on_line;
-      Eigen::Vector3f P = known_it->smooth.point_on_line + pol_invert.dot(ld_unit) * ld_unit;
-      Eigen::Vector3f x_axis(1,0,0);
-      known_it->smooth.bearing = acosf(x_axis.dot(P) / P.norm());
-      // we also want to encode the direction of the angle
-      if (P[1] < 0)  known_it->smooth.bearing = fabs(known_it->smooth.bearing)*-1.;
+      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;
     }
@@ -359,62 +312,36 @@ LaserLinesThread::loop()
   }
 
   for (LineInfo &l : linfos) {
-    TrackedLineInfo tl;
-    tl.history.set_capacity(cfg_moving_avg_window_size_);
-    tl.base_point = l.base_point;
-    tl.bearing = l.bearing;
-    tl.cloud = l.cloud;
-    tl.end_point_1 = l.end_point_1;
-    tl.end_point_2 = l.end_point_2;
-    tl.length = l.length;
-    tl.line_direction = l.line_direction;
-    tl.point_on_line = l.point_on_line;
-    fawkes::tf::Stamped<fawkes::tf::Point> bp_new(
-	  fawkes::tf::Point(
-	      l.base_point[0], l.base_point[1], l.base_point[2]
-	  ), fawkes::Time(0,0), finput_->header.frame_id);
-    tf_listener->transform_point("/odom", bp_new, tl.base_point_odom);
-    tl.history.push_back(l);
-
-    tl.smooth.base_point = l.base_point;
-    tl.smooth.bearing = l.bearing;
-    tl.smooth.cloud = l.cloud;
-    tl.smooth.end_point_1 = l.end_point_1;
-    tl.smooth.end_point_2 = l.end_point_2;
-    tl.smooth.length = l.length;
-    tl.smooth.line_direction = l.line_direction;
-    tl.smooth.point_on_line = l.point_on_line;
-
+    // 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);
   }
 
+  }
+
+  // When there are too many lines, delete the ones farthest away
   std::sort(known_lines_.begin(), known_lines_.end(),
-      [](const LineInfo &l1, const LineInfo &l2) -> bool
+      [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool
       {
-	Eigen::Vector3f x_axis(1,0,0);
-
-	Eigen::Vector3f l1_diff = l1.end_point_2 - l1.end_point_1;
-	Eigen::Vector3f l1_ctr = l1.end_point_1 + l1_diff / 2;
-	float l1_ctr_yaw = acosf(x_axis.dot(l1_ctr) / l1_ctr.norm());
-
-	Eigen::Vector3f l2_diff = l2.end_point_2 - l2.end_point_1;
-	Eigen::Vector3f l2_ctr = l2.end_point_1 + l2_diff / 2;
-	float l2_ctr_yaw = acosf(x_axis.dot(l2_ctr) / l2_ctr.norm());
-
-	return l1_ctr_yaw < l2_ctr_yaw;
-      });
-  }
+	return l1.raw.point_on_line.norm() < l2.raw.point_on_line.norm();
+      }
+  );
+  while (known_lines_.size() > cfg_max_num_lines_)
+    known_lines_.erase(known_lines_.end() - 1);
 
-  if (known_lines_.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.
-    known_lines_.resize(cfg_max_num_lines_);
-  }
+  // 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;
+      }
+  );
 
-  std::vector<LineInfo> linfos_filtered;
-  linfos_filtered.resize(known_lines_.size());
-    
   // set line parameters
   size_t oi = 0;
   unsigned int line_if_idx = 0;
@@ -422,7 +349,7 @@ LaserLinesThread::loop()
     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.smooth);
@@ -430,9 +357,9 @@ LaserLinesThread::loop()
       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;
@@ -448,17 +375,17 @@ 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
   { unsigned int line_id = 0;
   for (size_t i = 0; i < known_lines_.size(); i++) {
-    publish_visualization(known_lines_[i], i, line_id, "laser_lines");
+    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", "_avg");
diff --git a/src/plugins/laser-lines/line_info.h b/src/plugins/laser-lines/line_info.h
index 101d403..e77c154 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -28,6 +28,7 @@
 #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
@@ -53,11 +54,25 @@ class LineInfo {
 };
 
 
-class TrackedLineInfo : public LineInfo {
+class TrackedLineInfo {
 public:
-  LineInfo smooth;
-  boost::circular_buffer<LineInfo> history;
-  fawkes::tf::Stamped<fawkes::tf::Point> base_point_odom;
+  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