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