/*---------------------------------------------------------------
						resetUniform
 ---------------------------------------------------------------*/
void  CPosePDFParticlesExtended::resetUniform(
			  float x_min,  float x_max,
			  float y_min,  float y_max,
			  vector_float	state_min,
			  vector_float	state_max,
			  float phi_min,float phi_max,
			  int	particlesCount)
{
	MRPT_START

	ASSERT_( state_min.size() == state_max.size() );

	if (particlesCount>0)
	{
		clear();
		m_particles.resize(particlesCount);
		for (int i=0;i<particlesCount;i++)
			m_particles[i].d = new TExtendedCPose2D();
	}

	size_t		i,M = m_particles.size();
	for (i=0;i<M;i++)
	{
		m_particles[i].d->pose.x(  randomGenerator.drawUniform( x_min, x_max ));
		m_particles[i].d->pose.y(  randomGenerator.drawUniform( y_min, y_max ));
		m_particles[i].d->pose.phi(randomGenerator.drawUniform( phi_min, phi_max ));
		m_particles[i].d->state.resize(state_min.size());
		m_particles[i].d->state.resize(state_max.size());

		for (int k=0;k<state_min.size();k++)
			m_particles[i].d->state[k] = randomGenerator.drawUniform( state_min[k], state_max[k] );

		m_particles[i].log_w=0;
	}

	MRPT_END
}
      inline int unpack( vector_float const& real_float_coefficient,
		  vector_complex_float& complex_coefficient ){
	size_t n = std::max( real_float_coefficient.size(), complex_coefficient.size() );
	return gsl_fft_real_float_unpack( real_float_coefficient.data(), complex_coefficient.data(),
					  1, n ); }
      inline int unpack( vector_float const& real_coefficient, vector_complex_float& complex_coefficient,
			 size_t const stride ){
	size_t n = std::max( real_coefficient.size(), complex_coefficient.size() );
	return gsl_fft_halfcomplex_float_unpack( real_coefficient.data(), complex_coefficient.data(),
					   stride, n ); }
Beispiel #4
0
// Add / Modify a 2D ellipse
// x[0,1]: Mean
// y[0,1,2]: Covariance matrix (0,0),(1,1),(0,1)
void CWindowDialogPlots::plotEllipse(
	const vector_float &x,
	const vector_float &y,
	const std::string  &lineFormat,
	const std::string  &plotName,
	bool showName)
{
	mpCovarianceEllipse  *theLayer;

	if (x.size()!=3 || y.size()!=3)
	{
		cerr << "[CWindowDialogPlots::plotEllipse] vectors do not have expected size!!" << endl;
		return;
	}

	wxString    lyName = _U(plotName.c_str());
	bool        updateAtTheEnd = false; // If we update an existing layer, update manually to refresh the changes!

	// Already existing layer?
	mpLayer* existingLy = m_plot->GetLayerByName( lyName );

	if (existingLy)
	{
		// Assure the class:
		mpCovarianceEllipse  *lyPlotEllipse = static_cast<mpCovarianceEllipse*> ( existingLy );

		if (!lyPlotEllipse)
		{
			cerr << "[CWindowDialogPlots::plotEllipse] Plot name '" << plotName << "' is not of expected class mpCovarianceEllipse!."<< endl;
			return;
		}

		// Ok:
		theLayer = lyPlotEllipse;
		updateAtTheEnd = true;
	}
	else
	{
		// Create it:
		theLayer = new mpCovarianceEllipse( 1,1,0,2,32, lyName );
		m_plot->AddLayer( theLayer );
	}

	// Set data:
	theLayer->SetCovarianceMatrix(y[0],y[2],y[1]);
	theLayer->SetCoordinateBase(x[0],x[1]);
	theLayer->SetQuantiles(x[2]);
	theLayer->ShowName(showName);

	// Line style:
	// -------------------
	bool isContinuous=true;
	int  lineColor[] = {0,0,255};
	int  lineWidth = 1;
	int  lineStyle = wxSOLID;

	// parse string:
	if ( string::npos != lineFormat.find(".") )		{ isContinuous=false; }
	if ( string::npos != lineFormat.find("-") )		{ isContinuous=true; lineStyle = wxSOLID; }
	if ( string::npos != lineFormat.find(":") )		{ isContinuous=true; lineStyle = wxLONG_DASH; }

	if ( string::npos != lineFormat.find("r") )		{ lineColor[0]=0xFF; lineColor[1]=0x00; lineColor[2]=0x00; }
	if ( string::npos != lineFormat.find("g") )		{ lineColor[0]=0x00; lineColor[1]=0xFF; lineColor[2]=0x00; }
	if ( string::npos != lineFormat.find("b") )		{ lineColor[0]=0x00; lineColor[1]=0x00; lineColor[2]=0xFF; }
	if ( string::npos != lineFormat.find("k") )		{ lineColor[0]=0x00; lineColor[1]=0x00; lineColor[2]=0x00; }
	if ( string::npos != lineFormat.find("m") )		{ lineColor[0]=192; lineColor[1]=0; lineColor[2]=192; }
	if ( string::npos != lineFormat.find("c") )		{ lineColor[0]=0; lineColor[1]=192; lineColor[2]=192; }

	if ( string::npos != lineFormat.find("1") )		{ lineWidth=1; }
	if ( string::npos != lineFormat.find("2") )		{ lineWidth=2; }
	if ( string::npos != lineFormat.find("3") )		{ lineWidth=3; }
	if ( string::npos != lineFormat.find("4") )		{ lineWidth=4; }
	if ( string::npos != lineFormat.find("5") )		{ lineWidth=5; }
	if ( string::npos != lineFormat.find("6") )		{ lineWidth=6; }
	if ( string::npos != lineFormat.find("7") )		{ lineWidth=7; }
	if ( string::npos != lineFormat.find("8") )		{ lineWidth=8; }
	if ( string::npos != lineFormat.find("9") )		{ lineWidth=9; }

	theLayer->SetContinuity(isContinuous);

	wxPen  pen( wxColour(lineColor[0],lineColor[1],lineColor[2]), lineWidth, lineStyle );
	theLayer->SetPen(pen);

	if (updateAtTheEnd)
		m_plot->Refresh(false);
}
Beispiel #5
0
// Add / Modify a 2D plot using a MATLAB-like format string
void CWindowDialogPlots::plot(
	const vector_float &x,
	const vector_float &y,
	const std::string  &lineFormat,
	const std::string  &plotName)
{
	mpFXYVector  *theLayer;

	wxString    lyName = _U(plotName.c_str());
	bool        updateAtTheEnd = false; // If we update an existing layer, update manually to refresh the changes!

	// Already existing layer?
	mpLayer* existingLy = m_plot->GetLayerByName( lyName );

	if (existingLy)
	{
		// Assure the class:
		mpFXYVector  *lyPlot2D = static_cast<mpFXYVector*> ( existingLy );

		if (!lyPlot2D)
		{
			cerr << "[CWindowDialogPlots::plot] Plot name '" << plotName << "' is not of expected class mpFXYVector!."<< endl;
			return;
		}

		// Ok:
		theLayer = lyPlot2D;
		updateAtTheEnd = true;
	}
	else
	{
		// Create it:
		theLayer = new mpFXYVector( lyName );
		m_plot->AddLayer( theLayer );
	}

	// Set data:
	{
		std::vector<float> x_(x.size()),y_(x.size());
		::memcpy(&x_[0],&x[0],sizeof(x[0])*x_.size());
		::memcpy(&y_[0],&y[0],sizeof(y[0])*y_.size());
		theLayer->SetData( x_,y_ );
	}

	// Line style:
	// -------------------
	bool isContinuous=true;
	int  lineColor[] = {0,0,255};
	int  lineWidth = 1;
	int  lineStyle = wxSOLID;

	// parse string:
	if ( string::npos != lineFormat.find(".") )		{ isContinuous=false; }
	if ( string::npos != lineFormat.find("-") )		{ isContinuous=true; lineStyle = wxSOLID; }
	if ( string::npos != lineFormat.find(":") )		{ isContinuous=true; lineStyle = wxLONG_DASH; }

	if ( string::npos != lineFormat.find("r") )		{ lineColor[0]=0xFF; lineColor[1]=0x00; lineColor[2]=0x00; }
	if ( string::npos != lineFormat.find("g") )		{ lineColor[0]=0x00; lineColor[1]=0xFF; lineColor[2]=0x00; }
	if ( string::npos != lineFormat.find("b") )		{ lineColor[0]=0x00; lineColor[1]=0x00; lineColor[2]=0xFF; }
	if ( string::npos != lineFormat.find("k") )		{ lineColor[0]=0x00; lineColor[1]=0x00; lineColor[2]=0x00; }
	if ( string::npos != lineFormat.find("m") )		{ lineColor[0]=192; lineColor[1]=0; lineColor[2]=192; }
	if ( string::npos != lineFormat.find("c") )		{ lineColor[0]=0; lineColor[1]=192; lineColor[2]=192; }

	if ( string::npos != lineFormat.find("1") )		{ lineWidth=1; }
	if ( string::npos != lineFormat.find("2") )		{ lineWidth=2; }
	if ( string::npos != lineFormat.find("3") )		{ lineWidth=3; }
	if ( string::npos != lineFormat.find("4") )		{ lineWidth=4; }
	if ( string::npos != lineFormat.find("5") )		{ lineWidth=5; }
	if ( string::npos != lineFormat.find("6") )		{ lineWidth=6; }
	if ( string::npos != lineFormat.find("7") )		{ lineWidth=7; }
	if ( string::npos != lineFormat.find("8") )		{ lineWidth=8; }
	if ( string::npos != lineFormat.find("9") )		{ lineWidth=9; }

	theLayer->SetContinuity(isContinuous);

	wxPen  pen( wxColour(lineColor[0],lineColor[1],lineColor[2]), lineWidth, lineStyle );
	theLayer->SetPen(pen);

	theLayer->ShowName(false);

	if (updateAtTheEnd)
		m_plot->Refresh(false);

}