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