Beispiel #1
0
/*---------------------------------------------------------------
						squareErrorVector
  ---------------------------------------------------------------*/
void  TMatchingPairList::squareErrorVector(const CPose2D &q, vector_float &out_sqErrs ) const
{
	out_sqErrs.resize( size() );
	// *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]

	const float ccos = cos(q.phi());
	const float csin = sin(q.phi());
	const float qx   = q.x();
	const float qy   = q.y();
	float  xx, yy;		// Transformed points

	const_iterator 			corresp;
	vector_float::iterator	e_i;
	for (corresp=begin(), e_i = out_sqErrs.begin();corresp!=end();corresp++, e_i++)
	{
		xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
		yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
		*e_i = square( corresp->this_x - xx ) + square( corresp->this_y - yy );
	}
}
Beispiel #2
0
/*---------------------------------------------------------------
						squareErrorVector
  ---------------------------------------------------------------*/
void  TMatchingPairList::squareErrorVector(
	const CPose2D &q,
	vector_float &out_sqErrs,
	vector_float &xs,
	vector_float &ys ) const
{
	out_sqErrs.resize( size() );
	xs.resize( size() );
	ys.resize( size() );

	// *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]

	const float ccos = cos(q.phi());
	const float csin = sin(q.phi());
	const float qx   = q.x();
	const float qy   = q.y();

	const_iterator 			corresp;
	vector_float::iterator	e_i, xx, yy;
	for (corresp=begin(), e_i = out_sqErrs.begin(), xx = xs.begin(), yy = ys.begin();corresp!=end();corresp++, e_i++, xx++,yy++)
	{
		*xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
		*yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
		*e_i = square( corresp->this_x - *xx ) + square( corresp->this_y - *yy );
	}
}
/*---------------------------------------------------------------
						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
}
Beispiel #4
0
 /**
  * C++ version of gsl_blas_srotm().
  * @param X A vector
  * @param Y A vector
  * @param P An array
  * @return Error code on failure
  */
 int srotm( vector_float& X, vector_float& Y, float const P[] ){
   return gsl_blas_srotm( X.get(), Y.get(), P ); }
Beispiel #5
0
 /**
  * C++ version of gsl_blas_srot().
  * @param X A vector
  * @param Y A vector
  * @param c A constant
  * @param s A constant
  * @return Error code on failure
  */
 int srot( vector_float& X, vector_float& Y, float c, float s ){
   return gsl_blas_srot( X.get(), Y.get(), c, s );
 }
Beispiel #6
0
 /**
  * C++ version of gsl_blas_saxpy().
  * @param alpha A vector
  * @param X A vector
  * @param Y A vector
  * @return Error code on failure
  */
 int saxpy( float alpha, vector_float const& X, vector_float& Y ){
   return gsl_blas_saxpy( alpha, X.get(), Y.get() ); }
Beispiel #7
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);

}
Beispiel #8
0
 /**
  * C++ version of gsl_blas_dsdot().
  * @param X First vector
  * @param Y Second vector
  * @param result Vector product
  * @return Error code on failure
  */
 int dsdot( vector_float const& X, vector_float const& Y, double* result ){
   return gsl_blas_dsdot( X.get(), Y.get(), result ); }
Beispiel #9
0
 /**
  * C++ version of gsl_blas_sger().
  * @param alpha A constant
  * @param X A vector
  * @param Y A vector
  * @param A A matrix
  * @return Error code on failure
  */
 int sger( float alpha, vector_float const& X, vector_float const& Y, matrix_float& A ){
   return gsl_blas_sger( alpha, X.get(), Y.get(), A.get() ); }
Beispiel #10
0
    /**
     * C++ version of gsl_blas_strsv().
     * @param Uplo Upper or lower triangular
     * @param TransA Transpose type
     * @param Diag Diagonal type
     * @param A A matrix
     * @param X A vector
     * @return Error code on failure
     */
    int strsv( CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA,
	       CBLAS_DIAG_t Diag, matrix_float const& A, vector_float& X ){
      return gsl_blas_strsv( Uplo, TransA, Diag, A.get(), X.get() ); }
Beispiel #11
0
 /**
  * C++ version of gsl_blas_snrm2().
  * @param X A vector
  * @return The Euclidean norm
  */
 float snrm2( vector_float const& X ){ return gsl_blas_snrm2( X.get() ); }
Beispiel #12
0
      /**
       * C++ version of gsl_permute_vector_float_inverse().
       * @param p A permutation
       * @param v A vector
       * @return Error code on failure
       */
      inline int vector_float_inverse( permutation const& p, vector_float& v ){
	return gsl_permute_vector_float_inverse( p.get(), v.get() ); } 
      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 #15
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 #16
0
 /**
  * C++ version of gsl_blas_sscal().
  * @param alpha A constant
  * @param X A vector
  */
 void sscal( float alpha, vector_float& X ){ gsl_blas_sscal( alpha, X.get() ); }
Beispiel #17
0
    /**
     * C++ version of gsl_blas_sgemv().
     * @param TransA Transpose type
     * @param alpha A constant
     * @param A A matrix
     * @param X A vector
     * @param beta Another constant
     * @param Y A vector
     * @return Error code on failure
     */
    int sgemv( CBLAS_TRANSPOSE_t TransA, float alpha, matrix_float const& A,
	       vector_float const& X, float beta, vector_float& Y ){
      return gsl_blas_sgemv( TransA, alpha, A.get(), X.get(), beta, Y.get() ); }
Beispiel #18
0
 /**
  * C++ version of gsl_blas_sasum().
  * @param X A vector
  * @return The absolute sum of the elements
  */
 float sasum( vector_float const& X ){ return gsl_blas_sasum( X.get() ); }
Beispiel #19
0
    /**
     * C++ version of gsl_blas_ssymv().
     * @param Uplo Upper or lower triangular
     * @param alpha A constant
     * @param A A matrix
     * @param X A vector
     * @param beta Another constant
     * @param Y A vector
     * @return Error code on failure
     */
    int ssymv( CBLAS_UPLO_t Uplo, float alpha, matrix_float const& A,
	       vector_float const& X, float beta, vector_float& Y ){
      return gsl_blas_ssymv( Uplo, alpha, A.get(), X.get(), beta, Y.get() ); }
Beispiel #20
0
 /**
  * C++ version of gsl_blas_isamax().
  * @param X A vector
  * @return Index of the largest-magnitude element
  */
 CBLAS_INDEX_t isamax( vector_float const& X ){ return gsl_blas_isamax( X.get() ); }
Beispiel #21
0
    /**
     * C++ version of gsl_blas_ssyr2().
     * @param Uplo Upper or lower triangular
     * @param alpha A constant
     * @param X A vector
     * @param Y A vector
     * @param A A matrix
     * @return Error code on failure
     */
    int ssyr2( CBLAS_UPLO_t Uplo, float alpha, vector_float const& X, vector_float const& Y,
	       matrix_float& A ){ return gsl_blas_ssyr2( Uplo, alpha, X.get(), Y.get(), A.get() ); }
Beispiel #22
0
 /**
  * C++ version of gsl_blas_sswap().
  * @param X A vector
  * @param Y A vector
  * @return Error code on failure
  */
 int sswap( vector_float& X, vector_float& Y ){ return gsl_blas_sswap( X.get(), Y.get() ); }
Beispiel #23
0
 /**
  * C++ version of gsl_blas_sdot().
  * @param X First vector
  * @param Y Second vector
  * @param result Vector product
  * @return Error code on failure
  */
 int sdot( vector_float const& X, vector_float const& Y, float* result ){
   return gsl_blas_sdot( X.get(), Y.get(), result ); }
Beispiel #24
0
 /**
  * C++ version of gsl_blas_scopy().
  * @param X A vector
  * @param Y A vector
  * @return Error code on failure
  */
 int scopy( vector_float const& X, vector_float& Y ){ return gsl_blas_scopy( X.get(), Y.get() ); }