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  */
42  const string &input_frame_id,
43  const string &tracking_frame_id,
44  float cfg_switch_tolerance,
45  unsigned int cfg_moving_avg_len,
46  fawkes::Logger *logger,
47  string plugin_name)
48 : transformer(tfer),
49  input_frame_id(input_frame_id),
50  tracking_frame_id(tracking_frame_id),
51  cfg_switch_tolerance(cfg_switch_tolerance),
52  history(cfg_moving_avg_len),
53  bearing_center(0),
54  logger(logger),
55  plugin_name(plugin_name)
56 {}
57 
58 
59 /** Compute this line's distance from line info
60  * @param linfo line info
61  * @return the scalar distance between the two base points in meters.
62  */
63 btScalar TrackedLineInfo::distance(const LineInfo &linfo) const
64 {
66  fawkes::tf::Point(
67  linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
68  ), fawkes::Time(0,0), input_frame_id);
70  try {
71  transformer->transform_point(tracking_frame_id, bp_new, bp_odom_new);
72  } catch (fawkes::tf::TransformException &e) {
73  // Continue without tf, track in input frame instead. Warning follows on update() call.
74  bp_odom_new = bp_new;
75  }
76 
77  return (bp_odom_new - this->base_point_odom).length();
78 }
79 
80 
81 /** Update this line.
82  * @param linfo new info to consume
83  * This also updates moving averages for all fields.
84  */
86 {
87  this->raw = linfo;
89  fawkes::tf::Point(
90  linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
91  ), fawkes::Time(0,0), input_frame_id);
92  try {
94  } catch (fawkes::tf::TransformException &e) {
95  logger->log_warn(plugin_name.c_str(), "Can't transform to %s. Attempting to track in %s.",
96  tracking_frame_id.c_str(), input_frame_id.c_str());
97  this->base_point_odom = bp_new;
98  }
99  this->history.push_back(linfo);
100 
101  Eigen::Vector3f base_point_sum(0,0,0), end_point_1_sum(0,0,0),
102  end_point_2_sum(0,0,0), line_direction_sum(0,0,0), point_on_line_sum(0,0,0);
103  float length_sum(0);
104  for (LineInfo &l : this->history) {
105  base_point_sum += l.base_point;
106  end_point_1_sum += l.end_point_1;
107  end_point_2_sum += l.end_point_2;
108  line_direction_sum += l.line_direction;
109  point_on_line_sum += l.point_on_line;
110  length_sum += l.length;
111  }
112 
113  size_t sz = this->history.size();
114  this->smooth.base_point = base_point_sum / sz;
115  this->smooth.cloud = linfo.cloud;
116  this->smooth.end_point_1 = end_point_1_sum / sz;
117  this->smooth.end_point_2 = end_point_2_sum / sz;
118  this->smooth.length = length_sum / sz;
119  this->smooth.line_direction = line_direction_sum / sz;
120  this->smooth.point_on_line = point_on_line_sum / sz;
121 
122  Eigen::Vector3f x_axis(1,0,0);
123 
124  Eigen::Vector3f ld_unit = this->smooth.line_direction / this->smooth.line_direction.norm();
125  Eigen::Vector3f pol_invert = Eigen::Vector3f(0,0,0) - this->smooth.point_on_line;
126  Eigen::Vector3f P = this->smooth.point_on_line + pol_invert.dot(ld_unit) * ld_unit;
127  this->smooth.bearing = std::acos(x_axis.dot(P) / P.norm());
128  // we also want to encode the direction of the angle
129  if (P[1] < 0)
130  this->smooth.bearing = std::abs(this->smooth.bearing) * -1.;
131 
132  Eigen::Vector3f l_diff = raw.end_point_2 - raw.end_point_1;
133  Eigen::Vector3f l_ctr = raw.end_point_1 + l_diff / 2.;
134  this->bearing_center = std::acos(x_axis.dot(l_ctr) / l_ctr.norm());
135  if (l_ctr[1] < 0)
136  this->bearing_center = std::abs(this->bearing_center) * -1.;
137 }
138 
139 
140 
Line information container.
Definition: line_info.h:38
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Definition: line_info.h:42
std::string input_frame_id
Input frame ID of raw line infos (base_laser usually)
Definition: line_info.h:64
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.
Base class for fawkes tf exceptions.
Definition: exceptions.h:34
Eigen::Vector3f end_point_1
line segment end point
Definition: line_info.h:50
std::string tracking_frame_id
Track lines relative to this frame (e.g. odom helps compensate movement)
Definition: line_info.h:65
STL namespace.
Eigen::Vector3f end_point_2
line segment end point
Definition: line_info.h:51
A class for handling time.
Definition: time.h:91
fawkes::tf::Transformer * transformer
Transformer used to transform from input_frame_id_to odom.
Definition: line_info.h:63
Eigen::Vector3f base_point
optimized closest point on line
Definition: line_info.h:48
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
Definition: line_info.h:61
fawkes::Logger * logger
Logger pointer of the calling class.
Definition: line_info.h:69
std::string plugin_name
Plugin name of the calling class.
Definition: line_info.h:70
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Eigen::Vector3f line_direction
line direction vector
Definition: line_info.h:46
float length
length of the detecte line segment
Definition: line_info.h:43
Eigen::Vector3f point_on_line
point on line vector
Definition: line_info.h:45
boost::circular_buffer< LineInfo > history
history of raw line geometries for computing moving average
Definition: line_info.h:67
fawkes::tf::Stamped< fawkes::tf::Point > base_point_odom
last reference point (in odom frame) for line tracking
Definition: line_info.h:62
LineInfo raw
the latest geometry of this line, i.e. unfiltered
Definition: line_info.h:60
Coordinate transforms between any two frames in a system.
Definition: transformer.h:68
void update(LineInfo &new_linfo)
Update this line.
Definition: line_info.cpp:85
float bearing_center
Bearing towards line center, used to select lines "in front of us" when there.
Definition: line_info.h:68
btScalar distance(const LineInfo &linfo) const
Compute this line&#39;s distance from line info.
Definition: line_info.cpp:63
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Definition: line_info.h:53
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, std::string plugin_name)
Constructor.
Definition: line_info.cpp:40
Interface for logging.
Definition: logger.h:34