Example #1
0
void  CPosePDFParticles::copyFrom(const CPosePDF &o)
{
	MRPT_START

	CParticleList::iterator			itDest;
	CParticleList::const_iterator	itSrc;

	if (this == &o) return;		// It may be used sometimes

	if (o.GetRuntimeClass()==CLASS_ID(CPosePDFParticles))
	{
		const CPosePDFParticles	*pdf = dynamic_cast<const CPosePDFParticles*>( &o );
		ASSERT_(pdf);

		// Both are m_particles:
		m_particles = pdf->m_particles;
	}
	else
	if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussian))
	{
		const CPosePDFGaussian	*pdf = static_cast<const CPosePDFGaussian*>( &o );
		size_t M = m_particles.size();
		std::vector<CVectorDouble>			parts;
		std::vector<CVectorDouble>::iterator partsIt;

		randomGenerator.drawGaussianMultivariateMany(parts,M,pdf->cov);

		clearParticles();
		m_particles.resize(M);

		for ( itDest = m_particles.begin(),partsIt=parts.begin();itDest!=m_particles.end();++itDest,++partsIt )
		{
			itDest->log_w = 0;
			itDest->d.reset(new CPose2D(
				(pdf->mean.x() + (*partsIt)[0]),
				(pdf->mean.y() + (*partsIt)[1]),
				(pdf->mean.phi() + (*partsIt)[2]))
			);
			itDest->d->normalizePhi();
		}

	}

	MRPT_END
}
/*---------------------------------------------------------------
					inverse
 ---------------------------------------------------------------*/
void	 CPosePDFParticlesExtended::inverse(CPosePDF  &o) const
{
	ASSERT_( o.GetRuntimeClass() == CLASS_ID(CPosePDFParticlesExtended) );
	CPosePDFParticlesExtended	*out = (CPosePDFParticlesExtended*) &o;


	out->copyFrom( *this );
	static CPose2D		nullPose(0,0,0);

	for (unsigned int i=0;i<out->m_particles.size();i++)
		out->m_particles[i].d->pose = nullPose - out->m_particles[i].d->pose;

}
Example #3
0
/*---------------------------------------------------------------
						operator =
  ---------------------------------------------------------------*/
void  CPosePDFParticles::copyFrom(const CPosePDF &o)
{
	MRPT_START

	CParticleList::iterator			itDest;
	CParticleList::const_iterator	itSrc;

	if (this == &o) return;		// It may be used sometimes

	if (o.GetRuntimeClass()==CLASS_ID(CPosePDFParticles))
	{
		const CPosePDFParticles	*pdf = static_cast<const CPosePDFParticles*>( &o );

		// Both are m_particles:
		if (m_particles.size()==pdf->m_particles.size())
		{
			for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
				   itSrc!=pdf->m_particles.end();
			      itSrc++, itDest++ )
			{
				(*itDest->d) = (*itSrc->d);
				itDest->log_w = itSrc->log_w;
			}
		}
		else
		{
			for ( itDest = m_particles.begin();itDest!=m_particles.end();itDest++ )
				delete itDest->d;

			m_particles.resize( pdf->m_particles.size() );

			for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
				   itSrc!=pdf->m_particles.end();
			      itSrc++, itDest++ )
			{
				itDest->d = new CPose2D( *itSrc->d );
				itDest->log_w = itSrc->log_w;
			}
		}
	}
	else
	if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussian))
	{
		const CPosePDFGaussian	*pdf = static_cast<const CPosePDFGaussian*>( &o );
		size_t M = m_particles.size();
		std::vector<vector_double>			parts;
		std::vector<vector_double>::iterator partsIt;

		randomGenerator.drawGaussianMultivariateMany(parts,M,pdf->cov);

		clearParticles();
		m_particles.resize(M);

		for ( itDest = m_particles.begin(),partsIt=parts.begin();itDest!=m_particles.end();itDest++,partsIt++ )
		{
			itDest->log_w = 0;
            itDest->d = new CPose2D( ( pdf->mean.x() + (*partsIt)[0] ),
									 ( pdf->mean.y() + (*partsIt)[1] ),
                                     ( pdf->mean.phi() + (*partsIt)[2] ) );
			itDest->d->normalizePhi();
		}

	}

	MRPT_END
}
/*---------------------------------------------------------------
						operator =
  ---------------------------------------------------------------*/
void  CPosePDFParticlesExtended::copyFrom(const CPosePDF &o)
{
	MRPT_START

	CParticleList::iterator	itSrc,itDest;

	if (this == &o) return;		// It may be used sometimes

	if (o.GetRuntimeClass()==CLASS_ID(CPosePDFParticlesExtended))
	{
		CPosePDFParticlesExtended	*pdf = (CPosePDFParticlesExtended*) &o;

		// Both are particles:
		if (m_particles.size()==pdf->m_particles.size())
		{
			for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
				   itSrc!=pdf->m_particles.end();
			      itSrc++, itDest++ )
			{
				(*itDest->d) = (*itSrc->d);
				itDest->log_w = itSrc->log_w;
			}
		}
		else
		{
			for ( itDest = m_particles.begin();itDest!=m_particles.end();itDest++ )
				delete itDest->d;

			m_particles.resize( pdf->m_particles.size() );

			for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
				   itSrc!=pdf->m_particles.end();
			      itSrc++, itDest++ )
			{
				itDest->d = new TExtendedCPose2D( *itSrc->d );
				itDest->log_w = itSrc->log_w;
			}
		}
	}
	else
	if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussian))
	{
		CPosePDFGaussian	*pdf = (CPosePDFGaussian*) &o;
		int					M = (int)m_particles.size();
		std::vector<vector<double> >			parts;
		std::vector<vector<double> >::iterator partsIt;

		randomGenerator.drawGaussianMultivariateMany(parts,M,pdf->cov);

		for ( itDest = m_particles.begin();itDest!=m_particles.end();itDest++ )
				delete itDest->d;

		m_particles.clear();
		m_particles.resize(M);

		for ( itDest = m_particles.begin(),partsIt=parts.begin();itDest!=m_particles.end();itDest++,partsIt++ )
		{
			itDest->log_w = 0;
            itDest->d = new TExtendedCPose2D();
			itDest->d->pose.x( pdf->mean.x() + (*partsIt)[0] );
			itDest->d->pose.x( pdf->mean.y() + (*partsIt)[1] );
			itDest->d->pose.phi(  pdf->mean.phi() + (*partsIt)[2] );

			itDest->d->pose.normalizePhi();
		}

	}

	MRPT_END
}