/*---------------------------------------------------------------
resetUniform
---------------------------------------------------------------*/
void CPosePDFParticlesExtended::resetUniform(
float x_min, float x_max,
float y_min, float y_max,
CVectorFloat state_min,
CVectorFloat 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
}
// Aux function
void add_robotShape_to_setOfLines(
const CVectorFloat &shap_x_,
const CVectorFloat &shap_y_,
mrpt::opengl::CSetOfLines &gl_shape,
const mrpt::poses::CPose2D &origin = mrpt::poses::CPose2D () )
{
const int N = shap_x_.size();
if (N>=2 && N==shap_y_.size() )
{
// Transform coordinates:
CVectorDouble shap_x(N), shap_y(N),shap_z(N);
for (int i=0;i<N;i++) {
origin.composePoint(
shap_x_[i], shap_y_[i], 0,
shap_x[i], shap_y[i], shap_z[i]);
}
gl_shape.appendLine( shap_x[0], shap_y[0], shap_z[0], shap_x[1],shap_y[1],shap_z[1] );
for (int i=0;i<=shap_x.size();i++) {
const int idx = i % shap_x.size();
gl_shape.appendLineStrip( shap_x[idx],shap_y[idx], shap_z[idx]);
}
}
}
// 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 CVectorFloat &x,
const CVectorFloat &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);
}
// Add / Modify a 2D plot using a MATLAB-like format string
void CWindowDialogPlots::plot(
const CVectorFloat &x,
const CVectorFloat &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);
}
