C++ (Cpp) CPose3DPDF::GetRuntimeClass Examples

Example #1

File: CPose3DPDFParticles.cpp Project: 3660628/mrpt

/*---------------------------------------------------------------
						operator =
  ---------------------------------------------------------------*/
void  CPose3DPDFParticles::copyFrom(const CPose3DPDF &o)
{
	MRPT_START

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

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

	if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFParticles))
	{
		const CPose3DPDFParticles	*pdf = static_cast<const CPose3DPDFParticles*>( &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 CPose3D( *itSrc->d );
				itDest->log_w = itSrc->log_w;
			}
		}
	}
	else
	if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFGaussian))
	{
		THROW_EXCEPTION("TO DO!!");
	}

	MRPT_END
}

Example #2

File: CPose3DPDFParticles.cpp Project: Triocrossing/mrpt

void CPose3DPDFParticles::copyFrom(const CPose3DPDF& o)
{
	MRPT_START

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

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

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

		m_particles = pdf->m_particles;
	}
	else if (o.GetRuntimeClass() == CLASS_ID(CPose3DPDFGaussian))
	{
		THROW_EXCEPTION("TO DO!!");
	}

	MRPT_END
}

Example #3

File: CPose3DPDFParticles.cpp Project: Triocrossing/mrpt

/*---------------------------------------------------------------
					inverse
 ---------------------------------------------------------------*/
void CPose3DPDFParticles::inverse(CPose3DPDF& o) const
{
	MRPT_START
	ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles));
	CPose3DPDFParticles* out = static_cast<CPose3DPDFParticles*>(&o);

	// Prepare the output:
	out->copyFrom(*this);

	CPose3DPDFParticles::CParticleList::iterator it;
	CPose3D zero(0, 0, 0);

	for (it = out->m_particles.begin(); it != out->m_particles.end(); ++it)
		*it->d = zero - *it->d;

	MRPT_END
}

Example #4

File: CPose3DPDFParticles.cpp Project: Aharobot/mrpt

/*---------------------------------------------------------------
						operator =
  ---------------------------------------------------------------*/
void  CPose3DPDFParticles::copyFrom(const CPose3DPDF &o)
{
	MRPT_START

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

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

	if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFParticles))
	{
		const CPose3DPDFParticles	*pdf = static_cast<const CPose3DPDFParticles*>( &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 CPose3D( *itSrc->d );
				itDest->log_w = itSrc->log_w;
			}
		}
	}
	else
	if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFGaussian))
	{
		THROW_EXCEPTION("TO DO!!");
/*		CPose3DPDFGaussian	*pdf = (CPosePDFGaussian*) &o;
		int					M = (int)m_particles.size();
		std::vector<vector_float>			parts;
		std::vector<vector_float>::iterator partsIt;

		mrpt::random::randomNormalMultiDimensionalMany(pdf->cov,M,parts);

		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 CPose3D( ( pdf->mean.x + (*partsIt)[0] ),
									 ( pdf->mean.y + (*partsIt)[1] ),
                                     ( pdf->mean.phi + (*partsIt)[2] ) );
			itDest->d->normalizePhi();
		}
*/
	}

	MRPT_END
}