/*---------------------------------------------------------------
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 );
}
}
/*---------------------------------------------------------------
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 );
}
}
