/*---------------------------------------------------------------
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 );
}
}
/*---------------------------------------------------------------
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
}
/**
* 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 ); }
/**
* 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 );
}
/**
* 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() ); }
// 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);
}
/**
* 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 ); }
/**
* 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() ); }
/**
* 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() ); }
/**
* 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() ); }
/**
* 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 ); }
// 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);
}
/**
* 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() ); }
/**
* 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() ); }
/**
* 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() ); }
/**
* 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() ); }
/**
* 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() ); }
/**
* 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() ); }
/**
* 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() ); }
/**
* 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 ); }
/**
* 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() ); }