#include <linear_pose_filter_2d.h>

Public Member Functions
geometry_msgs::Pose	filter (const geometry_msgs::Pose &pose)
	Method to filter pose objects. Filter only filters position.x, position.y and the yaw of the pose. More...

	LinearPoseFilter2D (const std::vector< float > &b, const std::vector< float > &a)
	Constructor for LinearPoseFilter2D. More...

void	reset ()
	Method to reset the filter to ensure that if the filter is being reused, it is not corrupted by old values. Sets sample histories to the origin. More...

void	setCoeff (const std::vector< float > &b, const std::vector< float > &a)
	Method to set the filter coefficients. More...

void	setFilterState (const geometry_msgs::Pose &input_pose, const geometry_msgs::Pose &output_pose)
	Method to set the sample history of the filter to some state. All of the sample histories will be set to the parameterized values. More...

void	setFilterState (const std::vector< geometry_msgs::Pose > &input_poses, const std::vector< geometry_msgs::Pose > &output_poses)
	Method to set the sample history of the filter to some (set of) state(s). new_poses[0] is the oldest sample and new_poses[new_poses.size() - 1] is the newest sample. If the number of poses is greater than the length of the filter memory, only the newest poses will be used. If the number of poses is less than the length of the filter memory, the poses will be copied to the most recent history and the original samples will retain their original location in the buffer. Example: // New poses. np = [1, 2, 3] // Original poses. p = [9, 8, 7, 6, 5] // Poses after setFilterState p = [9, 8, 1, 2, 3]. More...

void	setFilterState (const geometry_msgs::Pose &input_pose)
	Not yet implemented but need to have methods that take only input histories and then recreate the appropriate output histories. This is because the input history encodes the entire filter history. More...

void	setFilterState (const std::vector< geometry_msgs::Pose > &input_poses)

Private Member Functions
float	getNewestOutputYaw ()
	Method to get the most recent filtered unnormalized yaw. If the filter has never produced an output before, the zero will be given. More...

float	getUnnormalizedYaw (geometry_msgs::Pose pose, float reference_yaw)
	Method to return the unnormalized/wrapped-up yaw. More...

geometry_msgs::Pose	originPose ()
	Method to get origin pose. More...

Private Attributes
std::vector< float >	a_
	Vector of input coefficients. More...

std::vector< float >	b_
	Vector of previous input yaws that aren't normalized. More...

std::deque< geometry_msgs::Pose >	in_
	Vector of previous output poses. More...

std::deque< geometry_msgs::Pose >	out_

std::deque< float >	yaw_in_
	Vector of previous output yaws that aren't normalized. More...

std::deque< float >	yaw_out_
	Vector of previous input poses. More...

Detailed Description

Definition at line 32 of file linear_pose_filter_2d.h.

Constructor & Destructor Documentation

LinearPoseFilter2D::LinearPoseFilter2D	(	const std::vector< float > &	b,
		const std::vector< float > &	a
	)

Constructor for LinearPoseFilter2D.

Parameters

b	Filter coefficients for the input samples. The number of coefficients is the order of the filter + 1.
a	Filter coefficients for the output samples. Vector a should have the same number of elements as b, with the first element being 1.

Definition at line 28 of file linear_pose_filter_2d.cpp.

Member Function Documentation

geometry_msgs::Pose LinearPoseFilter2D::filter ( const geometry_msgs::Pose & pose )

Method to filter pose objects. Filter only filters position.x, position.y and the yaw of the pose.

Parameters

pose	Input pose to be filtered.

Returns: Filtered pose.

Definition at line 74 of file linear_pose_filter_2d.cpp.

float LinearPoseFilter2D::getNewestOutputYaw ( )

private

Method to get the most recent filtered unnormalized yaw. If the filter has never produced an output before, the zero will be given.

Returns: Most recently filtered yaw.

Definition at line 229 of file linear_pose_filter_2d.cpp.

float LinearPoseFilter2D::getUnnormalizedYaw	(	geometry_msgs::Pose	pose,
		float	reference_yaw
	)

private

Method to return the unnormalized/wrapped-up yaw.

This method adds shortest angular distance between the pose and the reference yaw and returns it. Example: pose = -178 degrees, reference_yaw = 177 degrees, return = 182 degrees. pose = 175 degrees, reference_yaw = -170 degrees, return = -185 degrees.

Parameters

pose	The pose to be unnormalized.
reference_yaw	The yaw with which to reference and whose sign is respected.

Returns: Unnormalized yaw in radians.

Definition at line 220 of file linear_pose_filter_2d.cpp.

geometry_msgs::Pose LinearPoseFilter2D::originPose ( )

private

Method to get origin pose.

Returns: Pose with the linear pose set to the origin and the quaternion set to Roll, Pitch, Yaw set to zero.

Definition at line 206 of file linear_pose_filter_2d.cpp.

void LinearPoseFilter2D::reset ( )

Method to reset the filter to ensure that if the filter is being reused, it is not corrupted by old values. Sets sample histories to the origin.

Definition at line 119 of file linear_pose_filter_2d.cpp.

void LinearPoseFilter2D::setCoeff	(	const std::vector< float > &	b,
		const std::vector< float > &	a
	)

Method to set the filter coefficients.

Parameters

b	Filter coefficients for the input samples. The number of coefficients is the order of the filter + 1.
a	Filter coefficients for the output samples. Vector a should have the same number of elements as b, with the first element being 1.

Definition at line 33 of file linear_pose_filter_2d.cpp.

void LinearPoseFilter2D::setFilterState	(	const geometry_msgs::Pose &	input_pose,
		const geometry_msgs::Pose &	output_pose
	)

Method to set the sample history of the filter to some state. All of the sample histories will be set to the parameterized values.

Parameters

input_pose	The initial input pose that will be set for all previous inputs.
output_pose	The initial output pose that will be set for all previous outputs.

Definition at line 125 of file linear_pose_filter_2d.cpp.

void LinearPoseFilter2D::setFilterState	(	const std::vector< geometry_msgs::Pose > &	input_poses,
		const std::vector< geometry_msgs::Pose > &	output_poses
	)

Method to set the sample history of the filter to some (set of) state(s). new_poses[0] is the oldest sample and new_poses[new_poses.size() - 1] is the newest sample. If the number of poses is greater than the length of the filter memory, only the newest poses will be used. If the number of poses is less than the length of the filter memory, the poses will be copied to the most recent history and the original samples will retain their original location in the buffer. Example: // New poses. np = [1, 2, 3] // Original poses. p = [9, 8, 7, 6, 5] // Poses after setFilterState p = [9, 8, 1, 2, 3].

Parameters

input_poses	Set of input poses to set the input state time history to.
output_poses	Set of output poses to set the output state time history to.

Definition at line 138 of file linear_pose_filter_2d.cpp.

void LinearPoseFilter2D::setFilterState ( const geometry_msgs::Pose & input_pose )

Not yet implemented but need to have methods that take only input histories and then recreate the appropriate output histories. This is because the input history encodes the entire filter history.

void LinearPoseFilter2D::setFilterState ( const std::vector< geometry_msgs::Pose > & input_poses )

Member Data Documentation

std::vector<float> LinearPoseFilter2D::a_

private

Vector of input coefficients.

Definition at line 140 of file linear_pose_filter_2d.h.

std::vector<float> LinearPoseFilter2D::b_

private

Vector of previous input yaws that aren't normalized.

Definition at line 139 of file linear_pose_filter_2d.h.

std::deque<geometry_msgs::Pose> LinearPoseFilter2D::in_

private

Vector of previous output poses.

Definition at line 136 of file linear_pose_filter_2d.h.

std::deque<geometry_msgs::Pose> LinearPoseFilter2D::out_

private

Definition at line 135 of file linear_pose_filter_2d.h.

std::deque<float> LinearPoseFilter2D::yaw_in_

private

Vector of previous output yaws that aren't normalized.

Definition at line 138 of file linear_pose_filter_2d.h.

std::deque<float> LinearPoseFilter2D::yaw_out_

private

Vector of previous input poses.

Definition at line 137 of file linear_pose_filter_2d.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation