void SE_average<2>::append(const mrpt::poses::CPose2D& p, const double weight)
{
m_count += weight;
m_accum_x += weight * p.x();
m_accum_y += weight * p.y();
m_rot_part.append(p.phi(), weight);
}
void SE_average<2>::get_average(mrpt::poses::CPose2D &ret_mean) const
{
ASSERT_ABOVE_(m_count,0);
ret_mean.x( m_accum_x / m_count );
ret_mean.y( m_accum_y / m_count );
const_cast<SO_average<2>*>(&m_rot_part)->enable_exception_on_undeterminate = this->enable_exception_on_undeterminate;
ret_mean.phi ( m_rot_part.get_average() );
}
/*-------------------------------------------------------------
changeOdometry
-------------------------------------------------------------*/
void CActivMediaRobotBase::changeOdometry(const mrpt::poses::CPose2D &newOdometry)
{
#if MRPT_HAS_ARIA
ArPose pos_actual;
THE_ROBOT->lock();
pos_actual.setPose( newOdometry.x()*1000, newOdometry.y()*1000, RAD2DEG( newOdometry.phi() ) );
THE_ROBOT->setDeadReconPose( pos_actual );
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
void CPTG_RobotShape_Circular::add_robotShape_to_setOfLines(
mrpt::opengl::CSetOfLines &gl_shape,
const mrpt::poses::CPose2D &origin) const
{
const double R = m_robotRadius;
const int N = 17;
// Transform coordinates:
mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
for (int i = 0; i<N; i++) {
origin.composePoint(
R*cos(i * 2 * M_PI / (N - 1)), R*sin(i * 2 * M_PI / (N - 1)), 0,
shap_x[i], shap_y[i], shap_z[i]);
}
// Draw a "radius" to identify the "forward" orientation (phi=0)
gl_shape.appendLine(origin.x(), origin.y(), .0, shap_x[0], shap_y[0], shap_z[0]);
for (int i = 1; i <= shap_x.size(); i++) {
const int idx = i % shap_x.size();
gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
}
// Draw a "cross" to identify the robot center
gl_shape.appendLine(origin.x() - R*0.02, origin.y(), .0, origin.x() + R*0.02, origin.y(), .0);
gl_shape.appendLine(origin.x(), origin.y() - R*0.02, .0, origin.x(), origin.y() + R*0.02, .0);
}