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
25using 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 */
64btScalar
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);
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 */
87void
89{
90 if (visibility_history >= 0)
92 else
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 */
103void
105{
106 if (visibility_history <= 0)
108 else
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.