23 #include "line_info.h" 42 const string &input_frame_id,
43 const string &tracking_frame_id,
44 float cfg_switch_tolerance,
45 unsigned int cfg_moving_avg_len,
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),
55 plugin_name(plugin_name)
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);
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;
113 size_t sz = this->history.size();
122 Eigen::Vector3f x_axis(1,0,0);
134 this->
bearing_center = std::acos(x_axis.dot(l_ctr) / l_ctr.norm());
Line information container.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
std::string input_frame_id
Input frame ID of raw line infos (base_laser usually)
Eigen::Vector3f end_point_1
line segment end point
std::string tracking_frame_id
Track lines relative to this frame (e.g. odom helps compensate movement)
Eigen::Vector3f end_point_2
line segment end point
A class for handling time.
fawkes::tf::Transformer * transformer
Transformer used to transform from input_frame_id_to odom.
Eigen::Vector3f base_point
optimized closest point on line
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
fawkes::Logger * logger
Logger pointer of the calling class.
std::string plugin_name
Plugin name of the calling class.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Eigen::Vector3f line_direction
line direction vector
float length
length of the detecte line segment
Eigen::Vector3f point_on_line
point on line vector
boost::circular_buffer< LineInfo > history
history of raw line geometries for computing moving average
fawkes::tf::Stamped< fawkes::tf::Point > base_point_odom
last reference point (in odom frame) for line tracking
LineInfo raw
the latest geometry of this line, i.e. unfiltered
void update(LineInfo &new_linfo)
Update this line.
float bearing_center
Bearing towards line center, used to select lines "in front of us" when there.
btScalar distance(const LineInfo &linfo) const
Compute this line's distance from line info.
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
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.