常用算法函数

absoluteDifference函数：

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

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

absoluteSum

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

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