Fawkes API  Fawkes Development Version
line_info.cpp
1 
2 /***************************************************************************
3  * line_info.cpp - line info container
4  *
5  * Created: Tue Mar 17 11:13:24 2015 (re-factoring)
6  * Copyright 2011-2015 Tim Niemueller [www.niemueller.de]
7  * 2016 Victor MatarĂ©
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "line_info.h"
24 
25 using namespace std;
26 
27 /** @class TrackedLineInfo "line_info.h"
28  * Container for a line with tracking and smoothing info.
29  */
30 
31 /** Constructor.
32  * @param tfer tf transformer
33  * @param input_frame_id frame id of incoming data
34  * @param tracking_frame_id fixed frame in which to perform tracking
35  * @param cfg_switch_tolerance tolerance in m for when to assume a line ID switch
36  * @param cfg_moving_avg_len length of buffer for moving average
37  * @param logger logger for informational messages
38  * @param plugin_name component for informational messages
39  */
41  const string & input_frame_id,
42  const string & tracking_frame_id,
43  float cfg_switch_tolerance,
44  unsigned int cfg_moving_avg_len,
45  fawkes::Logger * logger,
46  const string & plugin_name)
47 : interface_idx(-1),
48  visibility_history(0),
49  transformer(tfer),
50  input_frame_id(input_frame_id),
51  tracking_frame_id(tracking_frame_id),
52  cfg_switch_tolerance(cfg_switch_tolerance),
53  history(cfg_moving_avg_len),
54  bearing_center(0),
55  logger(logger),
56  plugin_name(plugin_name)
57 {
58 }
59 
60 /** Compute this line's distance from line info
61  * @param linfo line info
62  * @return the scalar distance between the two base points in meters.
63  */
64 btScalar
66 {
67  fawkes::tf::Stamped<fawkes::tf::Point> bp_new(fawkes::tf::Point(linfo.base_point[0],
68  linfo.base_point[1],
69  linfo.base_point[2]),
70  fawkes::Time(0, 0),
73  try {
74  transformer->transform_point(tracking_frame_id, bp_new, bp_odom_new);
75  } catch (fawkes::tf::TransformException &e) {
76  // Continue without tf, track in input frame instead. Warning follows on update() call.
77  bp_odom_new = bp_new;
78  }
79 
80  return (bp_odom_new - this->base_point_odom).length();
81 }
82 
83 /**
84  * Update this currently not visible line, make the visibility history (more) negative
85  * and invalidate the data.
86  */
87 void
89 {
90  if (visibility_history >= 0)
91  visibility_history = -1;
92  else
93  visibility_history -= 1;
94 
95  this->raw.cloud.reset();
96  this->smooth.cloud.reset();
97 }
98 
99 /** Update this line.
100  * @param linfo new info to consume
101  * This also updates moving averages for all fields.
102  */
103 void
105 {
106  if (visibility_history <= 0)
107  visibility_history = 1;
108  else
109  visibility_history += 1;
110 
111  this->raw = linfo;
112  fawkes::tf::Stamped<fawkes::tf::Point> bp_new(fawkes::tf::Point(linfo.base_point[0],
113  linfo.base_point[1],
114  linfo.base_point[2]),
115  fawkes::Time(0, 0),
117  try {
119  } catch (fawkes::tf::TransformException &e) {
120  logger->log_warn(plugin_name.c_str(),
121  "Can't transform to %s. Attempting to track in %s.",
122  tracking_frame_id.c_str(),
123  input_frame_id.c_str());
124  this->base_point_odom = bp_new;
125  }
126  this->history.push_back(linfo);
127 
128  Eigen::Vector3f base_point_sum(0, 0, 0), end_point_1_sum(0, 0, 0), end_point_2_sum(0, 0, 0),
129  line_direction_sum(0, 0, 0), point_on_line_sum(0, 0, 0);
130  float length_sum(0);
131  for (LineInfo &l : this->history) {
132  base_point_sum += l.base_point;
133  end_point_1_sum += l.end_point_1;
134  end_point_2_sum += l.end_point_2;
135  line_direction_sum += l.line_direction;
136  point_on_line_sum += l.point_on_line;
137  length_sum += l.length;
138  }
139 
140  size_t sz = this->history.size();
141  this->smooth.base_point = base_point_sum / sz;
142  this->smooth.cloud = linfo.cloud;
143  this->smooth.end_point_1 = end_point_1_sum / sz;
144  this->smooth.end_point_2 = end_point_2_sum / sz;
145  this->smooth.length = length_sum / sz;
146  this->smooth.line_direction = line_direction_sum / sz;
147  this->smooth.point_on_line = point_on_line_sum / sz;
148 
149  Eigen::Vector3f x_axis(1, 0, 0);
150 
151  Eigen::Vector3f ld_unit = this->smooth.line_direction / this->smooth.line_direction.norm();
152  Eigen::Vector3f pol_invert = Eigen::Vector3f(0, 0, 0) - this->smooth.point_on_line;
153  Eigen::Vector3f P = this->smooth.point_on_line + pol_invert.dot(ld_unit) * ld_unit;
154  this->smooth.bearing = std::acos(x_axis.dot(P) / P.norm());
155  // we also want to encode the direction of the angle
156  if (P[1] < 0)
157  this->smooth.bearing = std::abs(this->smooth.bearing) * -1.;
158 
159  Eigen::Vector3f l_diff = raw.end_point_2 - raw.end_point_1;
160  Eigen::Vector3f l_ctr = raw.end_point_1 + l_diff / 2.;
161  this->bearing_center = std::acos(x_axis.dot(l_ctr) / l_ctr.norm());
162  if (l_ctr[1] < 0)
163  this->bearing_center = std::abs(this->bearing_center) * -1.;
164 }
Line information container.
Definition: line_info.h:40
Eigen::Vector3f point_on_line
point on line vector
Definition: line_info.h:47
float length
length of the detecte line segment
Definition: line_info.h:45
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Definition: line_info.h:55
Eigen::Vector3f line_direction
line direction vector
Definition: line_info.h:48
Eigen::Vector3f end_point_1
line segment end point
Definition: line_info.h:52
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Definition: line_info.h:44
Eigen::Vector3f base_point
optimized closest point on line
Definition: line_info.h:50
Eigen::Vector3f end_point_2
line segment end point
Definition: line_info.h:53
LineInfo raw
the latest geometry of this line, i.e. unfiltered
Definition: line_info.h:64
boost::circular_buffer< LineInfo > history
history of raw line geometries for computing moving average
Definition: line_info.h:75
void update(LineInfo &new_linfo)
Update this line.
Definition: line_info.cpp:104
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
Definition: line_info.h:65
btScalar distance(const LineInfo &linfo) const
Compute this line's distance from line info.
Definition: line_info.cpp:65
float bearing_center
Bearing towards line center, used to select lines "in front of us" when there.
Definition: line_info.h:77
std::string plugin_name
Plugin name of the calling class.
Definition: line_info.h:79
fawkes::tf::Transformer * transformer
Transformer used to transform from input_frame_id_to odom.
Definition: line_info.h:69
void not_visible_update()
Update this currently not visible line, make the visibility history (more) negative and invalidate th...
Definition: line_info.cpp:88
TrackedLineInfo(fawkes::tf::Transformer *tfer, const std::string &input_frame_id, const std::string &tracking_frame_id, float cfg_switch_tolerance, unsigned int cfg_moving_avg_len, fawkes::Logger *logger, const std::string &plugin_name)
Constructor.
Definition: line_info.cpp:40
int visibility_history
visibility history of this line, negative for "no sighting"
Definition: line_info.h:63
std::string input_frame_id
Input frame ID of raw line infos (base_laser usually)
Definition: line_info.h:70
std::string tracking_frame_id
Track lines relative to this frame (e.g. odom helps compensate movement)
Definition: line_info.h:72
fawkes::Logger * logger
Logger pointer of the calling class.
Definition: line_info.h:78
fawkes::tf::Stamped< fawkes::tf::Point > base_point_odom
last reference point (in odom frame) for line tracking
Definition: line_info.h:67
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
A class for handling time.
Definition: time.h:93
Base class for fawkes tf exceptions.
Definition: exceptions.h:31
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.