コード例 #1
0
ファイル: TMatchingPair.cpp プロジェクト: Aharobot/mrpt
/*---------------------------------------------------------------
						squareErrorVector
  ---------------------------------------------------------------*/
void  TMatchingPairList::squareErrorVector(
	const CPose2D &q,
	vector_float &out_sqErrs,
	vector_float &xs,
	vector_float &ys ) const
{
	out_sqErrs.resize( size() );
	xs.resize( size() );
	ys.resize( size() );

	// *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]

	const float ccos = cos(q.phi());
	const float csin = sin(q.phi());
	const float qx   = q.x();
	const float qy   = q.y();

	const_iterator 			corresp;
	vector_float::iterator	e_i, xx, yy;
	for (corresp=begin(), e_i = out_sqErrs.begin(), xx = xs.begin(), yy = ys.begin();corresp!=end();corresp++, e_i++, xx++,yy++)
	{
		*xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
		*yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
		*e_i = square( corresp->this_x - *xx ) + square( corresp->this_y - *yy );
	}
}
コード例 #2
0
ファイル: TMatchingPair.cpp プロジェクト: Aharobot/mrpt
/*---------------------------------------------------------------
						squareErrorVector
  ---------------------------------------------------------------*/
void  TMatchingPairList::squareErrorVector(const CPose2D &q, vector_float &out_sqErrs ) const
{
	out_sqErrs.resize( size() );
	// *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]

	const float ccos = cos(q.phi());
	const float csin = sin(q.phi());
	const float qx   = q.x();
	const float qy   = q.y();
	float  xx, yy;		// Transformed points

	const_iterator 			corresp;
	vector_float::iterator	e_i;
	for (corresp=begin(), e_i = out_sqErrs.begin();corresp!=end();corresp++, e_i++)
	{
		xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
		yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
		*e_i = square( corresp->this_x - xx ) + square( corresp->this_y - yy );
	}
}