36 size_t N = std::min(b.size(), a.size());;
44 std::copy_backward(b.end() - N, b.end(),
b_.end());
45 std::copy_backward(a.end() - N, a.end(),
a_.end());
49 int K =
in_.size() -
b_.size();
53 for (
int i = 0; i < K; i++)
64 for (
int i = 0; i > K; i--)
89 std::deque<geometry_msgs::Pose>::iterator outn = --
out_.end();
90 std::deque<float>::iterator yaw_outn = --
yaw_out_.end();
93 size_t N =
b_.size() - 1;
94 for (
size_t i = 0; i < (N+1); i++)
97 outn->position.x +=
b_[i]*
in_[N - i].position.x;
98 outn->position.y +=
b_[i]*
in_[N - i].position.y;
105 outn->position.x -=
a_[i]*
out_[N - i].position.x;
106 outn->position.y -=
a_[i]*
out_[N - i].position.y;
112 outn->orientation.z =
sin(*yaw_outn/2.0);
113 outn->orientation.w =
cos(*yaw_outn/2.0);
128 float yaw_out =
tf::getYaw(output_pose.orientation);
130 std::fill(
out_.begin(),
out_.end(), output_pose);
134 std::fill(
in_.begin(),
in_.end(), input_pose);
139 const std::vector<geometry_msgs::Pose>& output_poses)
144 std::vector<geometry_msgs::Pose>::const_iterator earliest_pose_out;
145 if (output_poses.size() <=
out_.size())
147 earliest_pose_out = output_poses.begin();
153 earliest_pose_out = output_poses.end() -
out_.size();
157 std::copy_backward(earliest_pose_out, output_poses.end(),
out_.end());
160 std::deque<float>::iterator yaw_out =
yaw_out_.end();
161 std::deque<float>::iterator yaw_previous =
yaw_out_.end();
162 std::vector<geometry_msgs::Pose>::const_iterator pose_out = output_poses.end();
163 while (pose_out != earliest_pose_out)
170 *(--yaw_previous) =
tf::getYaw((--output_poses.end())->orientation);
181 std::vector<geometry_msgs::Pose>::const_iterator earliest_pose_in;
182 if (input_poses.size() <=
in_.size())
184 earliest_pose_in = input_poses.begin();
190 earliest_pose_in = input_poses.end() -
in_.size();
194 std::copy_backward(earliest_pose_in, input_poses.end(),
in_.end());
198 std::deque<float>::iterator yaw_in =
yaw_in_.end();
199 std::vector<geometry_msgs::Pose>::const_iterator pose_in = input_poses.end();
200 while (pose_in != earliest_pose_in)
208 geometry_msgs::Pose origin;
209 origin.position.x = 0;
210 origin.position.y = 0;
211 origin.position.z = 0;
212 origin.orientation.x = 0;
213 origin.orientation.y = 0;
214 origin.orientation.z = 0;
215 origin.orientation.w = 1;
float getNewestOutputYaw()
Method to get the most recent filtered unnormalized yaw. If the filter has never produced an output b...
void reset()
Method to reset the filter to ensure that if the filter is being reused, it is not corrupted by old v...
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...
static double getYaw(const Quaternion &bt_q)
std::deque< geometry_msgs::Pose > in_
Vector of previous output poses.
float getUnnormalizedYaw(geometry_msgs::Pose pose, float reference_yaw)
Method to return the unnormalized/wrapped-up yaw.
std::deque< geometry_msgs::Pose > out_
std::deque< float > yaw_in_
Vector of previous output yaws that aren't normalized.
void setCoeff(const std::vector< float > &b, const std::vector< float > &a)
Method to set the filter coefficients.
std::vector< float > b_
Vector of previous input yaws that aren't normalized.
def normalize_angle(angle)
geometry_msgs::Pose originPose()
Method to get origin pose.
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...
std::vector< float > a_
Vector of input coefficients.
LinearPoseFilter2D(const std::vector< float > &b, const std::vector< float > &a)
Constructor for LinearPoseFilter2D.
std::deque< float > yaw_out_
Vector of previous input poses.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)