36 #define STATS_EXPERIMENT 0 37 #define STATS_EXPERIMENT_ALSO_NC 1 91 m_pkk.extractMatrix(0,0,COV);
92 out_robotPose.
cov = COV;
103 std::vector<TPoint2D> &out_landmarksPositions,
104 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
117 m_pkk.extractMatrix(0,0,COV);
118 out_robotPose.
cov = COV;
123 size_t i,nLMs = (
m_xkk.size() - 3) / 2;
124 out_landmarksPositions.resize(nLMs);
127 out_landmarksPositions[i].x =
m_xkk[3+i*2+0];
128 out_landmarksPositions[i].y =
m_xkk[3+i*2+1];
135 out_fullState.resize(
m_xkk.size() );
136 for (KFVector::Index i=0;i<
m_xkk.size();i++)
137 out_fullState[i] =
m_xkk[i];
140 out_fullCovariance =
m_pkk;
150 CActionCollectionPtr &action,
151 CSensoryFramePtr &SF )
171 CPosePDFGaussianPtr auxPosePDF = CPosePDFGaussian::Create();
186 CActionRobotMovement2DPtr actMov2D =
m_action->getBestMovementEstimation();
191 u[1]=actMov3D->poseChange.mean.y();
192 u[2]=actMov3D->poseChange.mean.yaw();
198 actMov2D->poseChange->getMean(estMovMean);
201 u[2]=estMovMean.
phi();
206 for (
size_t i=0;i<3;i++) u[i]=0;
214 const KFArray_ACT &u,
216 bool &out_skipPrediction )
const 224 out_skipPrediction =
true;
228 CPose2D robotPose(xv[0],xv[1],xv[2]);
229 CPose2D odoIncrement(u[0],u[1],u[2]);
232 robotPose = robotPose + odoIncrement;
236 xv[2]=robotPose.
phi();
248 CActionRobotMovement2DPtr act2D =
m_action->getBestMovementEstimation();
251 if (act3D && act2D)
THROW_EXCEPTION(
"Both 2D & 3D odometry are present!?!?")
261 Ap =
TPoint2D(act2D->poseChange->getMeanVal());
278 F.get_unsafe(0,2) = -Ax*sy-Ay*cy;
279 F.get_unsafe(1,2) = Ax*cy-Ay*sy;
291 CActionRobotMovement2DPtr act2D =
m_action->getBestMovementEstimation();
294 if (act3D && act2D)
THROW_EXCEPTION(
"Both 2D & 3D odometry are present!?!?")
298 if (!act3D && !act2D)
304 for (
size_t i=0;i<3;i++)
331 vector_KFArray_OBS &out_predictions )
const 337 ASSERTMSG_(obs,
"*ERROR*: This method requires an observation of type CObservationBearingRange")
338 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
363 const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
368 const size_t N = idx_landmarks_to_predict.
size();
369 out_predictions.resize(N);
370 for (
size_t i=0;i<N;i++)
372 const size_t idx_lm = idx_landmarks_to_predict[i];
376 const kftype xi =
m_xkk[ vehicle_size + feature_size*idx_lm + 0 ];
377 const kftype yi =
m_xkk[ vehicle_size + feature_size*idx_lm + 1 ];
379 const double Axi = xi-sensorPoseAbs.
x();
380 const double Ayi = yi-sensorPoseAbs.
y();
382 out_predictions[i][0] = sqrt(
square(Axi)+
square(Ayi) );
390 const size_t &idx_landmark_to_predict,
392 KFMatrix_OxF &Hy )
const 398 ASSERTMSG_(obs,
"*ERROR*: This method requires an observation of type CObservationBearingRange")
399 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
429 const kftype cphi0 = cos(phi0);
430 const kftype sphi0 = sin(phi0);
433 const kftype x0s = sensorPoseOnRobot.x();
434 const kftype y0s = sensorPoseOnRobot.y();
435 const kftype phis = sensorPoseOnRobot.phi();
437 const kftype cphi0s = cos(phi0+phis);
438 const kftype sphi0s = sin(phi0+phis);
440 const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
443 const kftype xi =
m_xkk[ vehicle_size + feature_size*idx_landmark_to_predict + 0 ];
444 const kftype yi =
m_xkk[ vehicle_size + feature_size*idx_landmark_to_predict + 1 ];
449 const kftype EXP1 = -2*yi*y0s*cphi0-2*yi*y0+2*xi*y0s*sphi0-2*xi*x0-2*xi*x0s*cphi0-2*yi*x0s*sphi0+2*y0s*y0*cphi0-2*y0s*x0*sphi0+2*y0*x0s*sphi0+
square(x0)+2*x0s*x0*cphi0+
square(x0s)+
square(y0s)+
square(xi)+
square(yi)+
square(y0);
452 const kftype EXP2 = cphi0s*xi+sphi0s*yi-sin(phis)*y0s-y0*sphi0s-x0s*cos(phis)-x0*cphi0s;
455 const kftype EXP3 = -sphi0s*xi+cphi0s*yi-cos(phis)*y0s-y0*cphi0s+x0s*sin(phis)+x0*sphi0s;
460 Hx.get_unsafe(0,0)= (-xi-sphi0*y0s+cphi0*x0s+x0)*sqrtEXP1_1;
461 Hx.get_unsafe(0,1)= (-yi+cphi0*y0s+y0+sphi0*x0s)*sqrtEXP1_1;
462 Hx.get_unsafe(0,2)= (y0s*xi*cphi0+y0s*yi*sphi0-y0*y0s*sphi0-x0*y0s*cphi0+x0s*xi*sphi0-x0s*yi*cphi0+y0*x0s*cphi0-x0s*x0*sphi0)*sqrtEXP1_1;
464 Hx.get_unsafe(1,0)= (sphi0s/(EXP2)+(EXP3)/EXP2sq*cphi0s) * EXP4;
465 Hx.get_unsafe(1,1)= (-cphi0s/(EXP2)+(EXP3)/EXP2sq*sphi0s) * EXP4;
466 Hx.get_unsafe(1,2)= ((-cphi0s*xi-sphi0s*yi+y0*sphi0s+x0*cphi0s)/(EXP2)-(EXP3)/EXP2sq*(-sphi0s*xi+cphi0s*yi-y0*cphi0s+x0*sphi0s)) * EXP4;
471 Hy.get_unsafe(0,0)= (xi+sphi0*y0s-cphi0*x0s-x0)*sqrtEXP1_1;
472 Hy.get_unsafe(0,1)= (yi-cphi0*y0s-y0-sphi0*x0s)*sqrtEXP1_1;
474 Hy.get_unsafe(1,0)= (-sphi0s/(EXP2)-(EXP3)/EXP2sq*cphi0s)* EXP4;
475 Hy.get_unsafe(1,1)= (cphi0s/(EXP2)-(EXP3)/EXP2sq*sphi0s) * EXP4;
492 vector_KFArray_OBS &Z,
494 const vector_KFArray_OBS &all_predictions,
497 const KFMatrix_OxO &
R 509 ASSERTMSG_(obs,
"*ERROR*: This method requires an observation of type CObservationBearingRange")
511 const size_t N = obs->sensedData.size();
515 for (
row=0,itObs = obs->sensedData.begin();itObs!=obs->sensedData.end();++itObs,++
row)
518 Z[
row][0] =itObs->range;
519 Z[
row][1] =itObs->yaw;
520 ASSERTMSG_(itObs->pitch==0,
"ERROR: Observation contains pitch!=0 but this is 2D-SLAM!!!")
525 data_association.assign(N,-1);
529 obs_idxs_needing_data_assoc.reserve(N);
535 for (
row=0,itObs = obs->sensedData.begin(),itDA=data_association.begin();itObs!=obs->sensedData.end();++itObs,++itDA,++
row)
538 if (itObs->landmarkID<0)
539 obs_idxs_needing_data_assoc.push_back(
row);
544 *itDA = itID->second;
551 if (obs_idxs_needing_data_assoc.empty())
557 for (
size_t idxObs=0;idxObs<data_association.size();idxObs++)
559 int da = data_association[idxObs];
564 #if STATS_EXPERIMENT // DEBUG: Generate statistic info 567 #if STATS_EXPERIMENT_ALSO_NC 573 for(itDA=data_association.begin();itDA!=data_association.end();++itDA,++idx_obs)
575 if (*itDA==-1)
continue;
576 const size_t lm_idx_in_map = *itDA;
577 size_t valid_idx_pred =
find_in_vector(lm_idx_in_map,lm_indices_in_S);
581 for (
size_t idx_pred=0;idx_pred<lm_indices_in_S.size();idx_pred++)
585 const size_t base_idx_in_S = obs_size*idx_pred;
587 S.extractMatrix(base_idx_in_S,base_idx_in_S,lm_cov);
592 if (valid_idx_pred==idx_pred)
593 fC.
printf(
"%e %e\n",md,log_pdf);
596 #if STATS_EXPERIMENT_ALSO_NC 597 fNC.
printf(
"%e %e\n",md,log_pdf);
609 const size_t nObsDA = obs_idxs_needing_data_assoc.size();
612 for (
size_t i=0;i<nObsDA;i++)
614 const size_t idx = obs_idxs_needing_data_assoc[i];
615 for (
unsigned k=0;k<obs_size;k++)
616 Z_obs_means.get_unsafe(i,k) =
Z[idx][k];
621 m_pkk.extractMatrix(0,0, Pxx);
625 const size_t nPredictions = lm_indices_in_S.size();
633 for (
size_t q=0;
q<nPredictions;
q++)
635 const size_t i = lm_indices_in_S[
q];
636 for (
size_t w=0;
w<obs_size;
w++)
662 data_association[ it->first ] = it->second;
723 std_sensor_range ( 0.1f),
724 std_sensor_yaw (
DEG2RAD( 0.5f )),
725 quantiles_3D_representation ( 3),
726 create_simplemap (false),
729 data_assoc_IC_chi2_thres (0.99),
731 data_assoc_IC_ml_threshold (0.0)
744 out.
printf(
"\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n");
748 out.
printf(
"data_assoc_IC_chi2_thres = %.06f\n", data_assoc_IC_chi2_thres );
750 out.
printf(
"data_assoc_IC_ml_threshold = %.06f\n", data_assoc_IC_ml_threshold );
783 ASSERTMSG_(obs,
"*ERROR*: This method requires an observation of type CObservationBearingRange")
784 const CPose2D sensorPoseOnRobot =
CPose2D( obs->sensorLocationOnRobot );
794 const kftype cphi0 = cos(phi0);
795 const kftype sphi0 = sin(phi0);
798 const kftype x0s = sensorPoseOnRobot.x();
799 const kftype y0s = sensorPoseOnRobot.y();
800 const kftype phis = sensorPoseOnRobot.phi();
802 const kftype hr = in_z[0];
803 const kftype ha = in_z[1];
805 const kftype cphi_0sa = cos(phi0+phis+ha);
806 const kftype sphi_0sa = sin(phi0+phis+ha);
809 yn[0] = hr*cphi_0sa+cphi0*x0s-sphi0*y0s+x0;
810 yn[1] = hr*sphi_0sa+sphi0*x0s+cphi0*y0s+y0;
813 dyn_dxv.get_unsafe(0,0) = 1;
814 dyn_dxv.get_unsafe(0,1) = 0;
815 dyn_dxv.get_unsafe(0,2) = -hr*sphi_0sa-sphi0*x0s-cphi0*y0s;
817 dyn_dxv.get_unsafe(1,0) = 0;
818 dyn_dxv.get_unsafe(1,1) = 1;
819 dyn_dxv.get_unsafe(1,2) = hr*cphi_0sa+cphi0*x0s-sphi0*y0s;
822 dyn_dhn.get_unsafe(0,0) = cphi_0sa;
823 dyn_dhn.get_unsafe(0,1) = -hr*sphi_0sa;
825 dyn_dhn.get_unsafe(1,0) = sphi_0sa;
826 dyn_dhn.get_unsafe(1,1) = hr*cphi_0sa;
838 const size_t in_obsIdx,
839 const size_t in_idxNewFeat )
844 ASSERTMSG_(obs,
"*ERROR*: This method requires an observation of type CObservationBearingRange")
849 ASSERT_( in_obsIdx < obs->sensedData.size() )
851 if (obs->sensedData[in_obsIdx].landmarkID>=0)
855 obs->sensedData[in_obsIdx].landmarkID,
888 m_pkk.extractMatrix(0,0,2,2, COV);
889 pointGauss.
cov = COV;
894 ellip->setPose(pointGauss.
mean);
895 ellip->setCovMatrix(pointGauss.
cov);
896 ellip->enableDrawSolid3D(
false);
898 ellip->set3DsegmentsCount(10);
899 ellip->setColor(1,0,0);
901 outObj->insert( ellip );
906 const size_t nLMs = (
m_xkk.size()-3)/2;
907 for (
size_t i=0;i<nLMs;i++)
911 m_pkk.extractMatrix(3+2*i,3+2*i,2,2, COV);
912 pointGauss.
cov = COV;
916 ellip->setName(
format(
"%u",static_cast<unsigned int>(i) ) );
917 ellip->enableShowName(
true);
918 ellip->setPose( pointGauss.
mean );
919 ellip->setCovMatrix( pointGauss.
cov );
920 ellip->enableDrawSolid3D(
false);
922 ellip->set3DsegmentsCount(10);
924 ellip->setColor(0,0,1);
926 outObj->insert( ellip );
938 const string &styleLandmarks,
939 const string &stylePath,
940 const string &styleRobot )
const 949 os::fprintf(f,
"%%--------------------------------------------------------------------\n");
950 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
951 os::fprintf(f,
"%% 'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'\n");
954 os::fprintf(f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
956 os::fprintf(f,
"%%--------------------------------------------------------------------\n");
986 CSensoryFramePtr dummySF;
1000 os::fprintf(f,
"plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n",stylePath.c_str());
1049 ASSERTMSG_(obs,
"*ERROR*: This method requires an observation of type CObservationBearingRange")
1051 const double sensor_max_range = obs->maxSensorDistance;
1052 const double fov_yaw = obs->fieldOfView_yaw;
1054 const double max_vehicle_loc_uncertainty = 4 * std::sqrt(
m_pkk.get_unsafe(0,0) +
m_pkk.get_unsafe(1,1) );
1055 const double max_vehicle_ang_uncertainty = 4 * std::sqrt(
m_pkk.get_unsafe(2,2) );
1057 out_LM_indices_to_predict.clear();
1058 for (
size_t i=0;i<prediction_means.size();i++)
1060 if ( prediction_means[i][0] < ( 1.5 + sensor_max_range + max_vehicle_loc_uncertainty + 4*
options.
std_sensor_range) &&
1065 out_LM_indices_to_predict.push_back(i);
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there i...
CPoint2D mean
The mean value.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
double x() const
Common members of all points & poses classes.
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
static CEllipsoidPtr Create()
std::string BASE_IMPEXP MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const size_t &nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose2D mean
The mean value.
CPose3D mean
The mean value.
void loadOptions(const mrpt::utils::CConfigFileBase &ini)
Load options from a ini-like file/text.
TOptions options
The options for the algorithm.
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
TDataAssociationMetric data_assoc_metric
const_iterator end() const
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
A gaussian distribution for 2D points.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
std::map< size_t, size_t > newly_inserted_landmarks
Map from the 0-based index within the last observation and the landmark 0-based index in the map (the...
bool create_simplemap
Whether to fill m_SFs (default=false)
size_t getNumberOfLandmarksInTheMap() const
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
const Scalar * const_iterator
KFMatrix m_pkk
The system full covariance matrix.
mrpt::utils::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
GLubyte GLubyte GLubyte GLubyte w
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
vector_KFArray_OBS all_predictions
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
std::map< observation_index_t, prediction_index_t > associations
For each observation (with row index IDX_obs in the input "Z_observations"), its association in the p...
Represents a probabilistic 3D (6D) movement.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
void OnObservationJacobiansNumericGetIncrements(KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
Only called if using a numeric approximation of the observation Jacobians, this method must return th...
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
This base provides a set of functions for maths stuff.
void getCurrentState(mrpt::poses::CPosePDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint2D > &out_landmarksPositions, std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_landmarkIDs, mrpt::math::CVectorDouble &out_fullState, mrpt::math::CMatrixDouble &out_fullCovariance) const
Returns the complete mean and cov.
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
void mahalanobisDistance2AndLogPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &log_pdf_out)
Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...
void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scala...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
static size_t get_vehicle_size()
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
This CStream derived class allow using a file as a write-only, binary stream.
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const
Returns a 3D representation of the landmarks in the map and the robot 3D position according to the cu...
float quantiles_3D_representation
Default = 3.
A helper class that can convert an enum value into its textual representation, and viceversa...
TDataAssociationMethod data_assoc_method
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
Information for data-association:
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
static size_t get_observation_size()
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const
If applicable to the given problem, this method implements the inverse observation model needed to ex...
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
const_iterator find_key(const KEY &k) const
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
mrpt::obs::CActionCollectionPtr m_action
Set up by processActionObservation.
std::map< KEY, VALUE >::iterator iterator
size_t size() const
Returns the count of pairs (pose,sensory data)
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
A class used to store a 3D point.
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, vector_size_t &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TDataAssocInfo m_last_data_association
Last data association.
double kftype
The numeric type used in the Kalman Filter (default=double)
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i'th pair, first one is index '0'.
CRangeBearingKFSLAM2D()
Default constructor.
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void OnNewLandmarkAddedToMap(const size_t in_obsIdx, const size_t in_idxNewFeat)
If applicable to the given problem, do here any special handling of adding a new landmark to the map...
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian &out_robotPose) const
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
float std_sensor_yaw
The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
GLenum GLenum GLvoid * row
TDataAssociationResults results
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
mrpt::obs::CSensoryFramePtr m_SF
Set up by processActionObservation.
GLsizei GLsizei GLchar * source
void OnObservationModel(const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
std::vector< size_t > vector_size_t
std::vector< int32_t > vector_int
TOptions()
Default values.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
size_t find_in_vector(const T &value, const CONTAINER &vect)
Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
static size_t get_feature_size()
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const vector_size_t &in_lm_indices_in_S, const KFMatrix_OxO &in_R)
This is called between the KF prediction step and the update step, and the application must return th...
#define ASSERTMSG_(f, __ERROR_MSG)
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
mrpt::vector_size_t predictions_IDs
KFVector m_xkk
The system state vector.
virtual ~CRangeBearingKFSLAM2D()
Destructor.
void saveMapAndPath2DRepresentationAsMATLABFile(const std::string &fil, float stdCount=3.0f, const std::string &styleLandmarks=std::string("b"), const std::string &stylePath=std::string("r"), const std::string &styleRobot=std::string("r")) const
Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the ele...
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
void SLAM_IMPEXP data_association_full_covariance(const mrpt::math::CMatrixDouble &Z_observations_mean, const mrpt::math::CMatrixDouble &Y_predictions_mean, const mrpt::math::CMatrixDouble &Y_predictions_cov, TDataAssociationResults &results, const TDataAssociationMethod method=assocJCBB, const TDataAssociationMetric metric=metricMaha, const double chi2quantile=0.99, const bool DAT_ASOC_USE_KDTREE=true, const std::vector< prediction_index_t > &predictions_IDs=std::vector< prediction_index_t >(), const TDataAssociationMetric compatibilityTestMetric=metricMaha, const double log_ML_compat_test_threshold=0.0)
Computes the data-association between the prediction of a set of landmarks and their observations...
void processActionObservation(mrpt::obs::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &SF)
Process one new action and observations to update the map and robot pose estimate.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void clear()
Remove all stored pairs.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.