Пример #1
0
void CEllipsoid_setFromPosePDF(CEllipsoid& self, CPose3DPDF& posePDF)
{
    CPose3D meanPose;
    CMatrixDouble66 COV;
    posePDF.getCovarianceAndMean(COV, meanPose);
    CMatrixDouble33 COV3 = COV.block(0,0,3,3);
    self.setLocation(meanPose.x(), meanPose.y(), meanPose.z() + 0.001 );
    self.setCovMatrix(COV3, COV3(2,2)==0 ? 2:3 );
}
	static CPose3DPDFGaussian  generateRandomPose3DPDF(double x,double y, double z, double yaw, double pitch, double roll, double std_scale)
	{
		CMatrixDouble61  r;
		mrpt::random::randomGenerator.drawGaussian1DMatrix(r, 0,std_scale);
		CMatrixDouble66 cov;
		cov.multiply_AAt(r);  // random semi-definite positive matrix:
		for (int i=0;i<6;i++) cov(i,i)+=1e-7;
		CPose3DPDFGaussian  p6pdf( CPose3D(x,y,z,yaw,pitch,roll), cov );
		return p6pdf;
	}
Пример #3
0
/*---------------------------------------------------------------
						copyFrom 3D
  ---------------------------------------------------------------*/
void  CPosePDFGaussian::copyFrom(const CPose3DPDF &o)
{
	// Convert to gaussian pdf:
	mean = CPose2D(o.getMeanVal());
	CMatrixDouble66 C;
	o.getCovariance(C);

	// Clip to 3x3:
	C(2,0)=C(0,2) = C(0,3);
	C(2,1)=C(1,2) = C(1,3);
	C(2,2)=         C(3,3);
	cov = C.block(0,0,3,3);
}
Пример #4
0
/*---------------------------------------------------------------
						setPosePDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPose3DPDF* pdf)
{
	MRPT_START

	clear();
	m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf->clone()));

	// According to the PDF type:
	if (IS_CLASS(m_pdf3D.get(), CPose3DPDFGaussian))
	{
		const CPose3DPDFGaussian* gPdf =
			static_cast<const CPose3DPDFGaussian*>(pdf);
		const CMatrixDouble66& cov = gPdf->cov;

		m_fastdraw_gauss_M_3D = gPdf->mean;

		/** Computes the eigenvalues/eigenvector decomposition of this matrix,
		*    so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
		*	  eigenvectors and the diagonal matrix D contains the eigenvalues
		*    as diagonal elements, sorted in <i>ascending</i> order.
		*/
		CMatrixDouble66 D;
		cov.eigenVectors(m_fastdraw_gauss_Z6, D);

		// Scale eigenvectors with eigenvalues:
		D = D.array().sqrt().matrix();
		m_fastdraw_gauss_Z6.multiply(m_fastdraw_gauss_Z6, D);
	}
	else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFParticles))
	{
		return;  // Nothing to prepare.
	}
	else
	{
		THROW_EXCEPTION_FMT(
			"Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
	}

	MRPT_END
}
Пример #5
0
/*---------------------------------------------------------------
						getCovarianceAndMean
  ---------------------------------------------------------------*/
void CPose3DPDFParticles::getCovarianceAndMean(
	CMatrixDouble66& cov, CPose3D& mean) const
{
	MRPT_START

	getMean(mean);  // First! the mean value:

	// Now the covariance:
	cov.zeros();
	CVectorDouble vars;
	vars.assign(6, 0.0);  // The diagonal of the final covariance matrix

	// Elements off the diagonal of the covariance matrix:
	double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
	double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
	double std_zya = 0, std_zp = 0, std_zr = 0;
	double std_yap = 0, std_yar = 0;
	double std_pr = 0;

	// Mean values in [0, 2pi] range:
	double mean_yaw = mean.yaw();
	double mean_pitch = mean.pitch();
	double mean_roll = mean.roll();
	if (mean_yaw < 0) mean_yaw += M_2PI;
	if (mean_pitch < 0) mean_pitch += M_2PI;
	if (mean_roll < 0) mean_roll += M_2PI;

	// Enought information to estimate the covariance?
	if (m_particles.size() < 2) return;

	// Sum all weight values:
	double W = 0;
	for (CPose3DPDFParticles::CParticleList::const_iterator it =
			 m_particles.begin();
		 it != m_particles.end(); ++it)
		W += exp(it->log_w);

	ASSERT_(W > 0);

	// Compute covariance:
	for (CPose3DPDFParticles::CParticleList::const_iterator it =
			 m_particles.begin();
		 it != m_particles.end(); ++it)
	{
		double w = exp(it->log_w) / W;

		// Manage 1 PI range:
		double err_yaw = wrapToPi(fabs(it->d->yaw() - mean_yaw));
		double err_pitch = wrapToPi(fabs(it->d->pitch() - mean_pitch));
		double err_roll = wrapToPi(fabs(it->d->roll() - mean_roll));

		double err_x = it->d->x() - mean.x();
		double err_y = it->d->y() - mean.y();
		double err_z = it->d->z() - mean.z();

		vars[0] += square(err_x) * w;
		vars[1] += square(err_y) * w;
		vars[2] += square(err_z) * w;
		vars[3] += square(err_yaw) * w;
		vars[4] += square(err_pitch) * w;
		vars[5] += square(err_roll) * w;

		std_xy += err_x * err_y * w;
		std_xz += err_x * err_z * w;
		std_xya += err_x * err_yaw * w;
		std_xp += err_x * err_pitch * w;
		std_xr += err_x * err_roll * w;

		std_yz += err_y * err_z * w;
		std_yya += err_y * err_yaw * w;
		std_yp += err_y * err_pitch * w;
		std_yr += err_y * err_roll * w;

		std_zya += err_z * err_yaw * w;
		std_zp += err_z * err_pitch * w;
		std_zr += err_z * err_roll * w;

		std_yap += err_yaw * err_pitch * w;
		std_yar += err_yaw * err_roll * w;

		std_pr += err_pitch * err_roll * w;
	}  // end for it

	// Unbiased estimation of variance:
	cov(0, 0) = vars[0];
	cov(1, 1) = vars[1];
	cov(2, 2) = vars[2];
	cov(3, 3) = vars[3];
	cov(4, 4) = vars[4];
	cov(5, 5) = vars[5];

	cov(1, 0) = cov(0, 1) = std_xy;
	cov(2, 0) = cov(0, 2) = std_xz;
	cov(3, 0) = cov(0, 3) = std_xya;
	cov(4, 0) = cov(0, 4) = std_xp;
	cov(5, 0) = cov(0, 5) = std_xr;

	cov(2, 1) = cov(1, 2) = std_yz;
	cov(3, 1) = cov(1, 3) = std_yya;
	cov(4, 1) = cov(1, 4) = std_yp;
	cov(5, 1) = cov(1, 5) = std_yr;

	cov(3, 2) = cov(2, 3) = std_zya;
	cov(4, 2) = cov(2, 4) = std_zp;
	cov(5, 2) = cov(2, 5) = std_zr;

	cov(4, 3) = cov(3, 4) = std_yap;
	cov(5, 3) = cov(3, 5) = std_yar;

	cov(5, 4) = cov(4, 5) = std_pr;

	MRPT_END
}