Ejemplo n.º 1
0
/*---------------------------------------------------------------
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 );
}
Ejemplo n.º 2
0
/*---------------------------------------------------------------
					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
}
Ejemplo n.º 3
0
double poses_test_compose2Dpoint(int a1, int a2)
{
	const long N = 500000;
	CTicTac	 tictac;

	CPose2D   a(1.0,2.0,DEG2RAD(10));
	CPoint2D  b(8.0,-5.0);

	CPoint2D p;
	for (long i=0;i<N;i++)
	{
		p = a+b;
	}
	double T = tictac.Tac()/N;
	dummy_do_nothing_with_string( mrpt::format("%f",p.x()) );
	return T;
}
Ejemplo n.º 4
0
	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;
	}
Ejemplo n.º 5
0
CPose2D::CPose2D(const CPoint2D &p) : m_phi(0),m_cossin_uptodate(false)
{
	m_coords[0] = p.x();
	m_coords[1] = p.y();
}
	float	R = randomGenerator.drawGaussian1D( m_dataPerBeacon[m_drawIndex].radiusAtRobotPlane, m_sigmaRanges);

	// 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);
Ejemplo n.º 7
0
// avoid headers include loops.
CPoint3D::CPoint3D(const CPoint2D& p)
{
	m_coords[0] = p.x();
	m_coords[1] = p.y();
	m_coords[2] = 0;
}
Ejemplo n.º 8
0
CPoint2D::CPoint2D(const CPoint2D &p)
{
    m_x = p.x();
    m_y = p.y();
}
Ejemplo n.º 9
0
double CPoint2D::measure_from(CPoint2D& o)
{
    return sqrt(pow((m_x - o.x()), 2) +
                pow((m_y - o.y()), 2));
}
Ejemplo n.º 10
0
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());
}
Ejemplo n.º 11
0
const CPoint2D CPoint2D::operator-(const CPoint2D &other) const
{
    return CPoint2D(m_x - other.x(), m_y - other.y());
}
Ejemplo n.º 12
0
const CPoint2D CPoint2D::operator+(const CPoint2D &other) const
{
    return CPoint2D(m_x + other.x(), m_y + other.y());
}