36 #define STATS_EXPERIMENT 0 37 #define STATS_EXPERIMENT_ALSO_NC 1 43 : options(), m_action(), m_SF(), m_IDs(), m_SFs()
94 std::vector<TPoint2D>& out_landmarksPositions,
95 std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
111 out_landmarksPositions.resize(nLMs);
112 for (i = 0; i < nLMs; i++)
114 out_landmarksPositions[i].x =
m_xkk[3 + i * 2 + 0];
115 out_landmarksPositions[i].y =
m_xkk[3 + i * 2 + 1];
126 out_fullCovariance =
m_pkk;
171 m_action->getBestMovementEstimation();
176 u[0] = actMov3D->poseChange.mean.x();
177 u[1] = actMov3D->poseChange.mean.y();
178 u[2] = actMov3D->poseChange.mean.yaw();
183 actMov2D->poseChange->getMean(estMovMean);
184 u[0] = estMovMean.
x();
185 u[1] = estMovMean.
y();
186 u[2] = estMovMean.
phi();
191 for (
size_t i = 0; i < 3; i++) u[i] = 0;
199 const KFArray_ACT& u, KFArray_VEH& xv,
bool& out_skipPrediction)
const 209 out_skipPrediction =
true;
213 CPose2D robotPose(xv[0], xv[1], xv[2]);
214 CPose2D odoIncrement(u[0], u[1], u[2]);
217 robotPose = robotPose + odoIncrement;
219 xv[0] = robotPose.
x();
220 xv[1] = robotPose.
y();
221 xv[2] = robotPose.
phi();
249 Ap =
TPoint2D(act2D->poseChange->getMeanVal().asTPose());
266 F(0, 2) = -Ax * sy - Ay * cy;
267 F(1, 2) = Ax * cy - Ay * sy;
289 if (!act3D && !act2D)
294 for (
size_t i = 0; i < 3; i++)
320 const std::vector<size_t>& idx_landmarks_to_predict,
321 vector_KFArray_OBS& out_predictions)
const 330 "*ERROR*: This method requires an observation of type " 331 "CObservationBearingRange");
332 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
359 const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
364 const size_t N = idx_landmarks_to_predict.size();
365 out_predictions.resize(N);
366 for (
size_t i = 0; i < N; i++)
368 const size_t idx_lm = idx_landmarks_to_predict[i];
372 const kftype xi =
m_xkk[vehicle_size + feature_size * idx_lm + 0];
373 const kftype yi =
m_xkk[vehicle_size + feature_size * idx_lm + 1];
375 const double Axi = xi - sensorPoseAbs.
x();
376 const double Ayi = yi - sensorPoseAbs.
y();
378 out_predictions[i][0] = sqrt(
square(Axi) +
square(Ayi));
379 out_predictions[i][1] =
387 size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy)
const 396 "*ERROR*: This method requires an observation of type " 397 "CObservationBearingRange");
398 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
430 const kftype cphi0 = cos(phi0);
431 const kftype sphi0 = sin(phi0);
434 const kftype x0s = sensorPoseOnRobot.
x();
435 const kftype y0s = sensorPoseOnRobot.
y();
436 const kftype phis = sensorPoseOnRobot.
phi();
438 const kftype cphi0s = cos(phi0 + phis);
439 const kftype sphi0s = sin(phi0 + phis);
441 const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
445 m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 0];
447 m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 1];
453 -2 * yi * y0s * cphi0 - 2 * yi * y0 + 2 * xi * y0s * sphi0 -
454 2 * xi * x0 - 2 * xi * x0s * cphi0 - 2 * yi * x0s * sphi0 +
455 2 * y0s * y0 * cphi0 - 2 * y0s * x0 * sphi0 + 2 * y0 * x0s * sphi0 +
460 const kftype EXP2 = cphi0s * xi + sphi0s * yi - sin(phis) * y0s -
461 y0 * sphi0s - x0s * cos(phis) - x0 * cphi0s;
464 const kftype EXP3 = -sphi0s * xi + cphi0s * yi - cos(phis) * y0s -
465 y0 * cphi0s + x0s * sin(phis) + x0 * sphi0s;
470 Hx(0, 0) = (-xi - sphi0 * y0s + cphi0 * x0s + x0) * sqrtEXP1_1;
471 Hx(0, 1) = (-yi + cphi0 * y0s + y0 + sphi0 * x0s) * sqrtEXP1_1;
472 Hx(0, 2) = (y0s * xi * cphi0 + y0s * yi * sphi0 - y0 * y0s * sphi0 -
473 x0 * y0s * cphi0 + x0s * xi * sphi0 - x0s * yi * cphi0 +
474 y0 * x0s * cphi0 - x0s * x0 * sphi0) *
477 Hx(1, 0) = (sphi0s / (EXP2) + (EXP3) / EXP2sq * cphi0s) * EXP4;
478 Hx(1, 1) = (-cphi0s / (EXP2) + (EXP3) / EXP2sq * sphi0s) * EXP4;
480 ((-cphi0s * xi - sphi0s * yi + y0 * sphi0s + x0 * cphi0s) / (EXP2) -
482 (-sphi0s * xi + cphi0s * yi - y0 * cphi0s + x0 * sphi0s)) *
488 Hy(0, 0) = (xi + sphi0 * y0s - cphi0 * x0s - x0) * sqrtEXP1_1;
489 Hy(0, 1) = (yi - cphi0 * y0s - y0 - sphi0 * x0s) * sqrtEXP1_1;
491 Hy(1, 0) = (-sphi0s / (EXP2) - (EXP3) / EXP2sq * cphi0s) * EXP4;
492 Hy(1, 1) = (cphi0s / (EXP2) - (EXP3) / EXP2sq * sphi0s) * EXP4;
520 vector_KFArray_OBS& Z, std::vector<int>& data_association,
521 const vector_KFArray_OBS& all_predictions,
const KFMatrix& S,
522 const std::vector<size_t>& lm_indices_in_S,
const KFMatrix_OxO&
R)
529 CObservationBearingRange::TMeasurementList::const_iterator itObs;
535 "*ERROR*: This method requires an observation of type " 536 "CObservationBearingRange");
538 const size_t N = obs->sensedData.size();
542 for (row = 0, itObs = obs->sensedData.begin();
543 itObs != obs->sensedData.end(); ++itObs, ++row)
546 Z[row][0] = itObs->range;
547 Z[row][1] = itObs->yaw;
550 "ERROR: Observation contains pitch!=0 but this is 2D-SLAM!!!");
555 data_association.assign(N, -1);
558 std::vector<size_t> obs_idxs_needing_data_assoc;
559 obs_idxs_needing_data_assoc.reserve(N);
562 std::vector<int>::iterator itDA;
563 for (row = 0, itObs = obs->sensedData.begin(),
564 itDA = data_association.begin();
565 itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
568 if (itObs->landmarkID < 0)
569 obs_idxs_needing_data_assoc.push_back(row);
575 *itDA = itID->second;
584 if (obs_idxs_needing_data_assoc.empty())
590 for (
size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
592 int da = data_association[idxObs];
595 data_association[idxObs];
598 #if STATS_EXPERIMENT // DEBUG: Generate statistic info 601 #if STATS_EXPERIMENT_ALSO_NC 605 std::vector<int>::iterator itDA;
607 for (itDA = data_association.begin();
608 itDA != data_association.end(); ++itDA, ++idx_obs)
610 if (*itDA == -1)
continue;
611 const size_t lm_idx_in_map = *itDA;
612 size_t valid_idx_pred =
617 for (
size_t idx_pred = 0; idx_pred < lm_indices_in_S.size();
621 all_predictions[lm_indices_in_S[idx_pred]];
623 const size_t base_idx_in_S = obs_size * idx_pred;
625 S.extractMatrix(base_idx_in_S, base_idx_in_S, lm_cov);
629 lm_mu - obs_mu, lm_cov, md, log_pdf);
631 if (valid_idx_pred == idx_pred)
632 fC.printf(
"%e %e\n", md, log_pdf);
635 #if STATS_EXPERIMENT_ALSO_NC 636 fNC.printf(
"%e %e\n", md, log_pdf);
647 const size_t nObsDA = obs_idxs_needing_data_assoc.size();
650 for (
size_t i = 0; i < nObsDA; i++)
652 const size_t idx = obs_idxs_needing_data_assoc[i];
653 for (
unsigned k = 0; k < obs_size; k++)
654 Z_obs_means(i, k) = Z[idx][k];
663 const size_t nPredictions = lm_indices_in_S.size();
671 for (
size_t q = 0; q < nPredictions; q++)
673 const size_t i = lm_indices_in_S[q];
674 for (
size_t w = 0; w < obs_size; w++)
676 all_predictions[i][w];
701 data_association[it->first] = it->second;
764 : stds_Q_no_odo(3, 0),
781 out <<
"\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n";
784 "data_assoc_method = %s\n",
788 "data_assoc_metric = %s\n",
792 "data_assoc_IC_chi2_thres = %.06f\n",
793 data_assoc_IC_chi2_thres);
795 "data_assoc_IC_metric = %s\n",
799 "data_assoc_IC_ml_threshold = %.06f\n",
800 data_assoc_IC_ml_threshold);
835 "*ERROR*: This method requires an observation of type " 836 "CObservationBearingRange");
837 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
847 const kftype cphi0 = cos(phi0);
848 const kftype sphi0 = sin(phi0);
851 const kftype x0s = sensorPoseOnRobot.
x();
852 const kftype y0s = sensorPoseOnRobot.
y();
853 const kftype phis = sensorPoseOnRobot.
phi();
855 const kftype hr = in_z[0];
856 const kftype ha = in_z[1];
858 const kftype cphi_0sa = cos(phi0 + phis + ha);
859 const kftype sphi_0sa = sin(phi0 + phis + ha);
862 yn[0] = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s + x0;
863 yn[1] = hr * sphi_0sa + sphi0 * x0s + cphi0 * y0s + y0;
868 dyn_dxv(0, 2) = -hr * sphi_0sa - sphi0 * x0s - cphi0 * y0s;
872 dyn_dxv(1, 2) = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s;
875 dyn_dhn(0, 0) = cphi_0sa;
876 dyn_dhn(0, 1) = -hr * sphi_0sa;
878 dyn_dhn(1, 0) = sphi_0sa;
879 dyn_dhn(1, 1) = hr * cphi_0sa;
895 const size_t in_obsIdx,
const size_t in_idxNewFeat)
903 "*ERROR*: This method requires an observation of type " 904 "CObservationBearingRange");
909 ASSERT_(in_obsIdx < obs->sensedData.size());
910 if (obs->sensedData[in_obsIdx].landmarkID >= 0)
913 m_IDs.
insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
949 ellip->setPose(pointGauss.
mean);
950 ellip->setCovMatrix(pointGauss.
cov);
951 ellip->enableDrawSolid3D(
false);
953 ellip->setColor(1, 0, 0);
955 outObj->insert(ellip);
959 const size_t nLMs = (
m_xkk.
size() - 3) / 2;
960 for (
size_t i = 0; i < nLMs; i++)
968 ellip->setName(
format(
"%u", static_cast<unsigned int>(i)));
969 ellip->enableShowName(
true);
970 ellip->setPose(pointGauss.
mean);
971 ellip->setCovMatrix(pointGauss.
cov);
972 ellip->enableDrawSolid3D(
false);
975 ellip->setColor(0, 0, 1);
977 outObj->insert(ellip);
985 const string& fil,
float stdCount,
const string& styleLandmarks,
986 const string& stylePath,
const string& styleRobot)
const 997 "%%--------------------------------------------------------------------" 999 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1003 "'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'" 1008 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1012 "%%--------------------------------------------------------------------" 1020 for (i = 0; i < nLMs; i++)
1024 cov(0, 0) =
m_pkk(idx + 0, idx + 0);
1025 cov(1, 1) =
m_pkk(idx + 1, idx + 1);
1059 f,
"plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1117 std::vector<size_t>& out_LM_indices_to_predict)
const 1123 "*ERROR*: This method requires an observation of type " 1124 "CObservationBearingRange");
1126 const double sensor_max_range = obs->maxSensorDistance;
1127 const double fov_yaw = obs->fieldOfView_yaw;
1129 const double max_vehicle_loc_uncertainty =
1131 const double max_vehicle_ang_uncertainty = 4 * std::sqrt(
m_pkk(2, 2));
1133 out_LM_indices_to_predict.clear();
1134 for (
size_t i = 0; i < prediction_means.size(); i++)
1138 if (prediction_means[i][0] <
1139 (1.5 + sensor_max_range + max_vehicle_loc_uncertainty +
1141 fabs(prediction_means[i][1]) <
1142 (20.0_deg + 0.5 * fov_yaw + max_vehicle_ang_uncertainty +
1146 out_LM_indices_to_predict.push_back(i);
1168 out_veh_increments[i] = 1e-6;
1170 out_feat_increments[i] = 1e-6;
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
const_iterator end() const
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
CPoint2D mean
The mean value.
CPose2D mean
The mean value.
void OnGetAction(KFArray_ACT &out_u) const override
Must return the action vector u.
void OnObservationJacobiansNumericGetIncrements(KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const override
Only called if using a numeric approximation of the observation Jacobians, this method must return th...
TOptions options
The options for the algorithm.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
TDataAssociationMetric data_assoc_metric
#define THROW_EXCEPTION(msg)
uint64_t TLandmarkID
Unique IDs for landmarks.
A gaussian distribution for 2D points.
std::string std::string format(std::string_view fmt, ARGS &&... args)
int void fclose(FILE *f)
An OS-independent version of fclose.
void OnObservationJacobians(size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .
void loadFromConfigFile(const mrpt::config::CConfigFileBase &iniFile, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
bool create_simplemap
Whether to fill m_SFs (default=false)
size_t getNumberOfLandmarksInTheMap() const
const_iterator find_key(const KEY &k) const
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
~CRangeBearingKFSLAM2D() override
Destructor.
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
static constexpr size_t get_feature_size()
mrpt::math::CVectorFixed< double, VEH_SIZE > KFArray_VEH
void OnNormalizeStateVector() override
This method is called after the prediction and after the update, to give the user an opportunity to n...
KFMatrix m_pkk
The system full covariance matrix.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
static Ptr Create(Args &&... args)
void clear()
Clear the contents of the bi-map.
mrpt::math::CMatrixDynamic< kftype > Y_pred_means
mrpt::math::TPose2D asTPose() const
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
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.
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
#define ASSERT_(f)
Defines an assertion mechanism.
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const override
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
Represents a probabilistic 3D (6D) movement.
void OnTransitionNoise(KFMatrix_VxV &out_Q) const override
Implements the transition noise covariance .
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
This class allows loading and storing values and vectors of different types from a configuration text...
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.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
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...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const override
If applicable to the given problem, this method implements the inverse observation model needed to ex...
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
double phi() const
Get the phi angle of the 2D pose (in radians)
float quantiles_3D_representation
Default = 3.
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:
constexpr double DEG2RAD(const double x)
Degrees to radians.
void OnTransitionJacobian(KFMatrix_VxV &out_F) const override
Implements the transition Jacobian .
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...
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
void assign(const std::size_t N, const Scalar value)
This namespace contains representation of robot actions and observations.
void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const override
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scala...
std::vector< size_t > predictions_IDs
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
mrpt::math::CMatrixDynamic< kftype > Y_pred_covs
double x() const
Common members of all points & poses classes.
size_t size() const
Returns the count of pairs (pose,sensory data)
double kftype
The numeric type used in the Kalman Filter (default=double)
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i'th pair, first one is index '0'.
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
mrpt::math::CVectorFixed< double, FEAT_SIZE > KFArray_FEAT
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
TDataAssocInfo m_last_data_association
Last data association.
return_t square(const num_t x)
Inline function for the square of a number.
#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...
static constexpr size_t get_observation_size()
CRangeBearingKFSLAM2D()
Default constructor.
std::vector< KFArray_OBS > vector_KFArray_OBS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const override
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R) override
This is called between the KF prediction step and the update step, and the application must return th...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::vision::TStereoCalibResults out
float std_sensor_range
The std.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
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...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
TDataAssociationResults results
constexpr double RAD2DEG(const double x)
Radians to degrees.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
double mean(const CONTAINER &v)
Computes the mean value of a vector.
mrpt::io::CFileOutputStream CFileOutputStream
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
static constexpr size_t get_vehicle_size()
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
void OnNewLandmarkAddedToMap(const size_t in_obsIdx, const size_t in_idxNewFeat) override
If applicable to the given problem, do here any special handling of adding a new landmark to the map...
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, float stdCount, const std::string &style=std::string("b"), size_t nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
TOptions()
Default values.
void resize(std::size_t N, bool zeroNewElements=false)
mrpt::math::CVectorFixed< double, OBS_SIZE > KFArray_OBS
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const override
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
Implements the transition model .
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
KFVector m_xkk
The system state vector.
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...
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const
Returns a 3D representation of the landmarks in the map and the robot 3D position according to the cu...
void 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::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &SF)
Process one new action and observations to update the map and robot pose estimate.
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
void clear()
Remove all stored pairs.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
void read_vector(const std::string §ion, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.