常用算法函数_atan2(sin(delta.theta), cos(delta.theta));-CSDN博客

本文链接：https://blog.csdn.net/qq_38441692/article/details/116700749

本文介绍了两个面向对象的模板函数：absoluteDifference用于计算两个位姿在目标坐标系下的差值，absoluteSum则用于在给定点系中求和。通过atan2和三角函数转换，函数实现了在不同坐标系间的坐标变换和位姿操作。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

absoluteDifference函数：

absoluteDifference()这个函数是将t时刻与t-1时刻里程计测量数据之差转换到t-1时刻的车辆坐标系下，计算位姿差

/*
@desc 两个位姿的差值，算出来P1在以P2为原点的坐标系里面的坐标。
*/
template <class T, class A>
orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
{
	orientedpoint<T,A> delta=p1-p2;
	delta.theta=atan2(sin(delta.theta), cos(delta.theta));
	double s=sin(p2.theta), c=cos(p2.theta);
	return orientedpoint<T,A>(c*delta.x+s*delta.y, 
	                         -s*delta.x+c*delta.y, delta.theta);
}

absoluteSum

计算p1的坐标加上p2的值之后的坐标

/** @brief p1的值加上p2的值，返回的值是在p1坐标系下的坐标
 */
template <class T, class A>
orientedpoint<T, A> absoluteSum(const orientedpoint<T, A>& p1,
                                const orientedpoint<T, A>& p2) {
  double s = sin(p1.theta), c = cos(p1.theta);
  orientedpoint<T, A> ans(c * p2.x - s * p2.y, s * p2.x + c * p2.y, p2.theta);
  ans = ans + p1;
  ans.normalize();
  return ans;
}