/*---------------------------------------------------------------
The operator u'="this"+u is the pose/point compounding operator.
---------------------------------------------------------------*/
CPoint2D CPose2D::operator + (const CPoint2D& u)const
{
update_cached_cos_sin();
return CPoint2D(
m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi );
}
/*---------------------------------------------------------------
drawSingleSample
---------------------------------------------------------------*/
void CPoint2DPDFGaussian::drawSingleSample(CPoint2D &outSample) const
{
MRPT_START
// Eigen3 emits an out-of-array warning here, but it seems to be a false warning? (WTF)
CVectorDouble vec;
randomGenerator.drawGaussianMultivariate(vec,cov);
ASSERT_(vec.size()==2);
outSample.x( mean.x() + vec[0] );
outSample.y( mean.y() + vec[1] );
MRPT_END
}
PointPosition CViewSegment2D::position( CPoint2D const& t_Point ) const {
auto aPosition = PointPosition::OnLine;
if ( !( t_Point.sameCoordinates( *m_StartPoint ) || t_Point.sameCoordinates( *m_EndPoint ) ) ) {
auto dx = m_EndPoint->x() - m_StartPoint->x();
auto dy = m_EndPoint->y() - m_StartPoint->y();
auto position = dx * ( t_Point.y() - m_StartPoint->y() ) - dy * ( t_Point.x() - m_StartPoint->x() );
if ( position > ViewerConstants::DISTANCE_TOLERANCE ) {
aPosition = PointPosition::Invisible;
}
else if ( position < -ViewerConstants::DISTANCE_TOLERANCE ) {
aPosition = PointPosition::Visible;
}
}
return aPosition;
}
CPose2D::CPose2D(const CPoint2D &p) : m_phi(0),m_cossin_uptodate(false)
{
m_coords[0] = p.x();
m_coords[1] = p.y();
}
// This is the point where the SENSOR is:
outSample.x( m_dataPerBeacon[m_drawIndex].beaconPosition.x + cos(ang) * R );
outSample.y( m_dataPerBeacon[m_drawIndex].beaconPosition.y + sin(ang) * R );
outSample.phi( randomGenerator.drawGaussian1D( m_oldPose.phi(), DEG2RAD(2) ) );
// Compute the robot pose P.
// P = SAMPLE - ROT · SENSOR_ON_ROBOT
CPoint2D on(m_dataPerBeacon[m_drawIndex].sensorOnRobot.x,m_dataPerBeacon[m_drawIndex].sensorOnRobot.y);
CPoint2D S(outSample.x(),outSample.y());
on = CPose2D(0,0,outSample.phi()) + on;
S = S - on;
outSample.x( S.x() );
outSample.y( S.y() );
MRPT_END
}
/*---------------------------------------------------------------
RS_observationLikelihood
---------------------------------------------------------------*/
double CRejectionSamplingRangeOnlyLocalization::RS_observationLikelihood( const CPose2D &x)
{
MRPT_START
double lik = 1.0;
double m_sigmaRanges2 = square(m_sigmaRanges);
// Evaluate the likelihood for all the observations but the "m_drawIndex":
// avoid headers include loops.
CPoint3D::CPoint3D(const CPoint2D& p)
{
m_coords[0] = p.x();
m_coords[1] = p.y();
m_coords[2] = 0;
}
CPoint2D::CPoint2D(const CPoint2D &p)
{
m_x = p.x();
m_y = p.y();
}
double CPoint2D::measure_from(CPoint2D& o)
{
return sqrt(pow((m_x - o.x()), 2) +
pow((m_y - o.y()), 2));
}
CPoint2D CPoint2D::rotate_about(CPoint2D& o, double rad)
{
return CPoint2D((m_x-o.x()) * cos(rad) - (m_y - o.y()) * sin(rad) + o.x(),
(m_y-o.y()) * cos(rad) + (m_x - o.x()) * sin(rad) + o.y());
}
const CPoint2D CPoint2D::operator-(const CPoint2D &other) const
{
return CPoint2D(m_x - other.x(), m_y - other.y());
}
const CPoint2D CPoint2D::operator+(const CPoint2D &other) const
{
return CPoint2D(m_x + other.x(), m_y + other.y());
}