Fawkes API
Fawkes Development Version
|
23 #include "line_info.h"
41 const string & input_frame_id,
42 const string & tracking_frame_id,
43 float cfg_switch_tolerance,
44 unsigned int cfg_moving_avg_len,
46 const string & plugin_name)
48 visibility_history(0),
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),
56 plugin_name(plugin_name)
121 "Can't transform to %s. Attempting to track in %s.",
126 this->
history.push_back(linfo);
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);
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;
140 size_t sz = this->history.size();
149 Eigen::Vector3f x_axis(1, 0, 0);
161 this->
bearing_center = std::acos(x_axis.dot(l_ctr) / l_ctr.norm());
Eigen::Vector3f line_direction
line direction vector
void update(LineInfo &new_linfo)
Update this line.
fawkes::tf::Stamped< fawkes::tf::Point > base_point_odom
last reference point (in odom frame) for line tracking
Eigen::Vector3f base_point
optimized closest point on line
std::string tracking_frame_id
Track lines relative to this frame (e.g. odom helps compensate movement)
int visibility_history
visibility history of this line, negative for "no sighting"
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.
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Eigen::Vector3f end_point_1
line segment end point
boost::circular_buffer< LineInfo > history
history of raw line geometries for computing moving average
std::string plugin_name
Plugin name of the calling class.
Line information container.
fawkes::Logger * logger
Logger pointer of the calling class.
std::string input_frame_id
Input frame ID of raw line infos (base_laser usually)
virtual void log_warn(const char *component, const char *format,...)=0
void not_visible_update()
Update this currently not visible line, make the visibility history (more) negative and invalidate th...
Eigen::Vector3f point_on_line
point on line vector
btScalar distance(const LineInfo &linfo) const
Compute this line's distance from line info.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
fawkes::tf::Transformer * transformer
Transformer used to transform from input_frame_id_to odom.
float length
length of the detecte line segment
Eigen::Vector3f end_point_2
line segment end point
float bearing_center
Bearing towards line center, used to select lines "in front of us" when there.
LineInfo raw
the latest geometry of this line, i.e. unfiltered