Fawkes API  Fawkes Development Version
line_info.h
1 
2 /***************************************************************************
3  * line_info.h - 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  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef _PLUGINS_LASER_LINES_LINE_INFO_H_
23 #define _PLUGINS_LASER_LINES_LINE_INFO_H_
24 
25 #include <logging/logger.h>
26 #include <pcl/point_cloud.h>
27 #include <pcl/point_types.h>
28 #include <tf/transformer.h>
29 #include <tf/types.h>
30 
31 #include <Eigen/Geometry>
32 #include <boost/circular_buffer.hpp>
33 #include <memory>
34 
35 /** Line information container.
36  * All points and angles are in the sensor reference frame
37  * from which the lines were extracted.
38  */
39 class LineInfo
40 {
41 public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
44  float bearing; ///< bearing to point on line
45  float length; ///< length of the detecte line segment
46 
47  Eigen::Vector3f point_on_line; ///< point on line vector
48  Eigen::Vector3f line_direction; ///< line direction vector
49 
50  Eigen::Vector3f base_point; ///< optimized closest point on line
51 
52  Eigen::Vector3f end_point_1; ///< line segment end point
53  Eigen::Vector3f end_point_2; ///< line segment end point
54 
55  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; ///< point cloud consisting only of
56  ///< points account to this line
57 };
58 
59 class TrackedLineInfo
60 {
61 public:
62  int interface_idx; ///< id of the interface, this line is written to, -1 when not yet assigned
63  int visibility_history; ///< visibility history of this line, negative for "no sighting"
64  LineInfo raw; ///< the latest geometry of this line, i.e. unfiltered
65  LineInfo smooth; ///< moving-average geometry of this line (cf. length of history buffer)
67  base_point_odom; ///< last reference point (in odom frame) for line tracking
69  * transformer; ///< Transformer used to transform from input_frame_id_to odom
70  std::string input_frame_id; ///< Input frame ID of raw line infos (base_laser usually)
71  std::string
72  tracking_frame_id; ///< Track lines relative to this frame (e.g. odom helps compensate movement)
73  float cfg_switch_tolerance; ///< Configured line jitter threshold
74  boost::circular_buffer<LineInfo>
75  history; ///< history of raw line geometries for computing moving average
76  float
77  bearing_center; ///< Bearing towards line center, used to select lines "in front of us" when there
78  fawkes::Logger *logger; ///< Logger pointer of the calling class
79  std::string plugin_name; ///< Plugin name of the calling class
80 
82  const std::string & input_frame_id,
83  const std::string & tracking_frame_id,
85  unsigned int cfg_moving_avg_len,
87  const std::string & plugin_name);
88 
89  btScalar distance(const LineInfo &linfo) const;
90  void update(LineInfo &new_linfo);
91  void not_visible_update();
92 };
93 
94 #endif
LineInfo::line_direction
Eigen::Vector3f line_direction
line direction vector
Definition: line_info.h:51
TrackedLineInfo::update
void update(LineInfo &new_linfo)
Update this line.
Definition: line_info.cpp:103
TrackedLineInfo::cfg_switch_tolerance
float cfg_switch_tolerance
Configured line jitter threshold.
Definition: line_info.h:72
TrackedLineInfo::base_point_odom
fawkes::tf::Stamped< fawkes::tf::Point > base_point_odom
last reference point (in odom frame) for line tracking
Definition: line_info.h:66
LineInfo::base_point
Eigen::Vector3f base_point
optimized closest point on line
Definition: line_info.h:53
TrackedLineInfo::tracking_frame_id
std::string tracking_frame_id
Track lines relative to this frame (e.g. odom helps compensate movement)
Definition: line_info.h:71
fawkes::tf::Transformer
Definition: transformer.h:71
TrackedLineInfo::visibility_history
int visibility_history
visibility history of this line, negative for "no sighting"
Definition: line_info.h:62
TrackedLineInfo::TrackedLineInfo
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:39
TrackedLineInfo::smooth
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
Definition: line_info.h:64
LineInfo::cloud
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Definition: line_info.h:58
fawkes::tf::Stamped< fawkes::tf::Point >
LineInfo::end_point_1
Eigen::Vector3f end_point_1
line segment end point
Definition: line_info.h:55
TrackedLineInfo::history
boost::circular_buffer< LineInfo > history
history of raw line geometries for computing moving average
Definition: line_info.h:74
fawkes::Logger
Definition: logger.h:40
TrackedLineInfo::plugin_name
std::string plugin_name
Plugin name of the calling class.
Definition: line_info.h:78
LineInfo
Line information container.
Definition: line_info.h:38
TrackedLineInfo::logger
fawkes::Logger * logger
Logger pointer of the calling class.
Definition: line_info.h:77
pcl::PointCloud
Definition: pointcloud.h:36
TrackedLineInfo::input_frame_id
std::string input_frame_id
Input frame ID of raw line infos (base_laser usually)
Definition: line_info.h:69
TrackedLineInfo::not_visible_update
void not_visible_update()
Update this currently not visible line, make the visibility history (more) negative and invalidate th...
Definition: line_info.cpp:87
LineInfo::point_on_line
Eigen::Vector3f point_on_line
point on line vector
Definition: line_info.h:50
TrackedLineInfo::distance
btScalar distance(const LineInfo &linfo) const
Compute this line's distance from line info.
Definition: line_info.cpp:64
LineInfo::bearing
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Definition: line_info.h:47
TrackedLineInfo
Definition: line_info.h:58
TrackedLineInfo::interface_idx
int interface_idx
id of the interface, this line is written to, -1 when not yet assigned
Definition: line_info.h:61
TrackedLineInfo::transformer
fawkes::tf::Transformer * transformer
Transformer used to transform from input_frame_id_to odom.
Definition: line_info.h:68
LineInfo::length
float length
length of the detecte line segment
Definition: line_info.h:48
LineInfo::end_point_2
Eigen::Vector3f end_point_2
line segment end point
Definition: line_info.h:56
TrackedLineInfo::bearing_center
float bearing_center
Bearing towards line center, used to select lines "in front of us" when there.
Definition: line_info.h:76
TrackedLineInfo::raw
LineInfo raw
the latest geometry of this line, i.e. unfiltered
Definition: line_info.h:63