Go to the documentation of this file.
35 #define STATS_EXPERIMENT 0
36 #define STATS_EXPERIMENT_ALSO_NC 1
42 : options(), m_action(), m_SF(), m_IDs(), m_SFs()
84 m_pkk.extractMatrix(0, 0, COV);
85 out_robotPose.
cov = COV;
95 std::vector<TPoint2D>& out_landmarksPositions,
96 std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
108 m_pkk.extractMatrix(0, 0, COV);
109 out_robotPose.
cov = COV;
113 size_t i, nLMs = (
m_xkk.size() - 3) / 2;
114 out_landmarksPositions.resize(nLMs);
115 for (i = 0; i < nLMs; i++)
117 out_landmarksPositions[i].x =
m_xkk[3 + i * 2 + 0];
118 out_landmarksPositions[i].y =
m_xkk[3 + i * 2 + 1];
125 out_fullState.resize(
m_xkk.size());
126 for (KFVector::Index i = 0; i <
m_xkk.size(); i++)
127 out_fullState[i] =
m_xkk[i];
130 out_fullCovariance =
m_pkk;
161 mrpt::make_aligned_shared<CPosePDFGaussian>();
176 m_action->getBestMovementEstimation();
182 u[1] = actMov3D->poseChange.mean.y();
183 u[2] = actMov3D->poseChange.mean.yaw();
188 actMov2D->poseChange->getMean(estMovMean);
189 u[0] = estMovMean.
x();
190 u[1] = estMovMean.
y();
191 u[2] = estMovMean.
phi();
196 for (
size_t i = 0; i < 3; i++) u[i] = 0;
204 const KFArray_ACT& u, KFArray_VEH& xv,
bool& out_skipPrediction)
const
214 out_skipPrediction =
true;
218 CPose2D robotPose(xv[0], xv[1], xv[2]);
219 CPose2D odoIncrement(u[0], u[1], u[2]);
222 robotPose = robotPose + odoIncrement;
224 xv[0] = robotPose.
x();
225 xv[1] = robotPose.
y();
226 xv[2] = robotPose.
phi();
254 Ap =
TPoint2D(act2D->poseChange->getMeanVal().asTPose());
271 F.get_unsafe(0, 2) = -Ax * sy - Ay * cy;
272 F.get_unsafe(1, 2) = Ax * cy - Ay * sy;
294 if (!act3D && !act2D)
299 for (
size_t i = 0; i < 3; i++)
325 const std::vector<size_t>& idx_landmarks_to_predict,
326 vector_KFArray_OBS& out_predictions)
const
335 "*ERROR*: This method requires an observation of type "
336 "CObservationBearingRange");
337 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
364 const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
369 const size_t N = idx_landmarks_to_predict.size();
370 out_predictions.resize(N);
371 for (
size_t i = 0; i < N; i++)
373 const size_t idx_lm = idx_landmarks_to_predict[i];
377 const kftype xi =
m_xkk[vehicle_size + feature_size * idx_lm + 0];
378 const kftype yi =
m_xkk[vehicle_size + feature_size * idx_lm + 1];
380 const double Axi = xi - sensorPoseAbs.
x();
381 const double Ayi = yi - sensorPoseAbs.
y();
383 out_predictions[i][0] = sqrt(
square(Axi) +
square(Ayi));
384 out_predictions[i][1] =
392 const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
393 KFMatrix_OxF& Hy)
const
402 "*ERROR*: This method requires an observation of type "
403 "CObservationBearingRange");
404 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
436 const kftype cphi0 = cos(phi0);
437 const kftype sphi0 = sin(phi0);
440 const kftype x0s = sensorPoseOnRobot.
x();
441 const kftype y0s = sensorPoseOnRobot.
y();
442 const kftype phis = sensorPoseOnRobot.
phi();
444 const kftype cphi0s = cos(phi0 + phis);
445 const kftype sphi0s = sin(phi0 + phis);
447 const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
451 m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 0];
453 m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 1];
459 -2 * yi * y0s * cphi0 - 2 * yi * y0 + 2 * xi * y0s * sphi0 -
460 2 * xi * x0 - 2 * xi * x0s * cphi0 - 2 * yi * x0s * sphi0 +
461 2 * y0s * y0 * cphi0 - 2 * y0s * x0 * sphi0 + 2 * y0 * x0s * sphi0 +
466 const kftype EXP2 = cphi0s * xi + sphi0s * yi - sin(phis) * y0s -
467 y0 * sphi0s - x0s * cos(phis) - x0 * cphi0s;
470 const kftype EXP3 = -sphi0s * xi + cphi0s * yi - cos(phis) * y0s -
471 y0 * cphi0s + x0s * sin(phis) + x0 * sphi0s;
476 Hx.get_unsafe(0, 0) = (-xi - sphi0 * y0s + cphi0 * x0s + x0) * sqrtEXP1_1;
477 Hx.get_unsafe(0, 1) = (-yi + cphi0 * y0s + y0 + sphi0 * x0s) * sqrtEXP1_1;
478 Hx.get_unsafe(0, 2) =
479 (y0s * xi * cphi0 + y0s * yi * sphi0 - y0 * y0s * sphi0 -
480 x0 * y0s * cphi0 + x0s * xi * sphi0 - x0s * yi * cphi0 +
481 y0 * x0s * cphi0 - x0s * x0 * sphi0) *
484 Hx.get_unsafe(1, 0) = (sphi0s / (EXP2) + (EXP3) / EXP2sq * cphi0s) * EXP4;
485 Hx.get_unsafe(1, 1) = (-cphi0s / (EXP2) + (EXP3) / EXP2sq * sphi0s) * EXP4;
486 Hx.get_unsafe(1, 2) =
487 ((-cphi0s * xi - sphi0s * yi + y0 * sphi0s + x0 * cphi0s) / (EXP2) -
489 (-sphi0s * xi + cphi0s * yi - y0 * cphi0s + x0 * sphi0s)) *
495 Hy.get_unsafe(0, 0) = (xi + sphi0 * y0s - cphi0 * x0s - x0) * sqrtEXP1_1;
496 Hy.get_unsafe(0, 1) = (yi - cphi0 * y0s - y0 - sphi0 * x0s) * sqrtEXP1_1;
498 Hy.get_unsafe(1, 0) = (-sphi0s / (EXP2) - (EXP3) / EXP2sq * cphi0s) * EXP4;
499 Hy.get_unsafe(1, 1) = (cphi0s / (EXP2) - (EXP3) / EXP2sq * sphi0s) * EXP4;
527 vector_KFArray_OBS& Z, std::vector<int>& data_association,
528 const vector_KFArray_OBS& all_predictions,
const KFMatrix& S,
529 const std::vector<size_t>& lm_indices_in_S,
const KFMatrix_OxO&
R)
543 "*ERROR*: This method requires an observation of type "
544 "CObservationBearingRange");
546 const size_t N = obs->sensedData.size();
550 for (
row = 0, itObs = obs->sensedData.begin();
551 itObs != obs->sensedData.end(); ++itObs, ++
row)
554 Z[
row][0] = itObs->range;
555 Z[
row][1] = itObs->yaw;
558 "ERROR: Observation contains pitch!=0 but this is 2D-SLAM!!!");
563 data_association.assign(N, -1);
566 std::vector<size_t> obs_idxs_needing_data_assoc;
567 obs_idxs_needing_data_assoc.reserve(N);
573 for (
row = 0, itObs = obs->sensedData.begin(),
574 itDA = data_association.begin();
575 itObs != obs->sensedData.end(); ++itObs, ++itDA, ++
row)
578 if (itObs->landmarkID < 0)
579 obs_idxs_needing_data_assoc.push_back(
row);
585 *itDA = itID->second;
594 if (obs_idxs_needing_data_assoc.empty())
600 for (
size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
602 int da = data_association[idxObs];
605 data_association[idxObs];
608 #if STATS_EXPERIMENT // DEBUG: Generate statistic info
611 #if STATS_EXPERIMENT_ALSO_NC
617 for (itDA = data_association.begin();
618 itDA != data_association.end(); ++itDA, ++idx_obs)
620 if (*itDA == -1)
continue;
621 const size_t lm_idx_in_map = *itDA;
622 size_t valid_idx_pred =
627 for (
size_t idx_pred = 0; idx_pred < lm_indices_in_S.size();
633 const size_t base_idx_in_S = obs_size * idx_pred;
635 S.extractMatrix(base_idx_in_S, base_idx_in_S, lm_cov);
639 lm_mu - obs_mu, lm_cov, md, log_pdf);
641 if (valid_idx_pred == idx_pred)
642 fC.printf(
"%e %e\n", md, log_pdf);
645 #if STATS_EXPERIMENT_ALSO_NC
646 fNC.printf(
"%e %e\n", md, log_pdf);
657 const size_t nObsDA = obs_idxs_needing_data_assoc.size();
660 for (
size_t i = 0; i < nObsDA; i++)
662 const size_t idx = obs_idxs_needing_data_assoc[i];
663 for (
unsigned k = 0; k < obs_size; k++)
664 Z_obs_means.get_unsafe(i, k) =
Z[idx][k];
669 m_pkk.extractMatrix(0, 0, Pxx);
673 const size_t nPredictions = lm_indices_in_S.size();
681 for (
size_t q = 0;
q < nPredictions;
q++)
683 const size_t i = lm_indices_in_S[
q];
684 for (
size_t w = 0;
w < obs_size;
w++)
712 data_association[it->first] = it->second;
776 : stds_Q_no_odo(3, 0),
777 std_sensor_range(0.1f),
779 quantiles_3D_representation(3),
780 create_simplemap(false),
783 data_assoc_IC_chi2_thres(0.99),
785 data_assoc_IC_ml_threshold(0.0)
801 "\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n");
804 "data_assoc_method = %s\n",
808 "data_assoc_metric = %s\n",
812 "data_assoc_IC_chi2_thres = %.06f\n",
813 data_assoc_IC_chi2_thres);
815 "data_assoc_IC_metric = %s\n",
819 "data_assoc_IC_ml_threshold = %.06f\n",
820 data_assoc_IC_ml_threshold);
855 "*ERROR*: This method requires an observation of type "
856 "CObservationBearingRange");
857 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
867 const kftype cphi0 = cos(phi0);
868 const kftype sphi0 = sin(phi0);
871 const kftype x0s = sensorPoseOnRobot.
x();
872 const kftype y0s = sensorPoseOnRobot.
y();
873 const kftype phis = sensorPoseOnRobot.
phi();
875 const kftype hr = in_z[0];
876 const kftype ha = in_z[1];
878 const kftype cphi_0sa = cos(phi0 + phis + ha);
879 const kftype sphi_0sa = sin(phi0 + phis + ha);
882 yn[0] = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s + x0;
883 yn[1] = hr * sphi_0sa + sphi0 * x0s + cphi0 * y0s + y0;
886 dyn_dxv.get_unsafe(0, 0) = 1;
887 dyn_dxv.get_unsafe(0, 1) = 0;
888 dyn_dxv.get_unsafe(0, 2) = -hr * sphi_0sa - sphi0 * x0s - cphi0 * y0s;
890 dyn_dxv.get_unsafe(1, 0) = 0;
891 dyn_dxv.get_unsafe(1, 1) = 1;
892 dyn_dxv.get_unsafe(1, 2) = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s;
895 dyn_dhn.get_unsafe(0, 0) = cphi_0sa;
896 dyn_dhn.get_unsafe(0, 1) = -hr * sphi_0sa;
898 dyn_dhn.get_unsafe(1, 0) = sphi_0sa;
899 dyn_dhn.get_unsafe(1, 1) = hr * cphi_0sa;
915 const size_t in_obsIdx,
const size_t in_idxNewFeat)
923 "*ERROR*: This method requires an observation of type "
924 "CObservationBearingRange");
929 ASSERT_(in_obsIdx < obs->sensedData.size());
930 if (obs->sensedData[in_obsIdx].landmarkID >= 0)
933 m_IDs.
insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
965 m_pkk.extractMatrix(0, 0, 2, 2, COV);
966 pointGauss.
cov = COV;
970 mrpt::make_aligned_shared<opengl::CEllipsoid>();
972 ellip->setPose(pointGauss.
mean);
973 ellip->setCovMatrix(pointGauss.
cov);
974 ellip->enableDrawSolid3D(
false);
976 ellip->set3DsegmentsCount(10);
977 ellip->setColor(1, 0, 0);
979 outObj->insert(ellip);
983 const size_t nLMs = (
m_xkk.size() - 3) / 2;
984 for (
size_t i = 0; i < nLMs; i++)
988 m_pkk.extractMatrix(3 + 2 * i, 3 + 2 * i, 2, 2, COV);
989 pointGauss.
cov = COV;
992 mrpt::make_aligned_shared<opengl::CEllipsoid>();
994 ellip->setName(
format(
"%u",
static_cast<unsigned int>(i)));
995 ellip->enableShowName(
true);
996 ellip->setPose(pointGauss.
mean);
997 ellip->setCovMatrix(pointGauss.
cov);
998 ellip->enableDrawSolid3D(
false);
1000 ellip->set3DsegmentsCount(10);
1002 ellip->setColor(0, 0, 1);
1004 outObj->insert(ellip);
1012 const string& fil,
float stdCount,
const string& styleLandmarks,
1013 const string& stylePath,
const string& styleRobot)
const
1024 "%%--------------------------------------------------------------------"
1026 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1030 "'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'"
1035 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1039 "%%--------------------------------------------------------------------"
1047 for (i = 0; i < nLMs; i++)
1051 cov(0, 0) =
m_pkk(idx + 0, idx + 0);
1052 cov(1, 1) =
m_pkk(idx + 1, idx + 1);
1086 f,
"plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1144 std::vector<size_t>& out_LM_indices_to_predict)
const
1150 "*ERROR*: This method requires an observation of type "
1151 "CObservationBearingRange");
1153 const double sensor_max_range = obs->maxSensorDistance;
1154 const double fov_yaw = obs->fieldOfView_yaw;
1156 const double max_vehicle_loc_uncertainty =
1157 4 * std::sqrt(
m_pkk.get_unsafe(0, 0) +
m_pkk.get_unsafe(1, 1));
1158 const double max_vehicle_ang_uncertainty =
1159 4 * std::sqrt(
m_pkk.get_unsafe(2, 2));
1161 out_LM_indices_to_predict.clear();
1162 for (
size_t i = 0; i < prediction_means.size(); i++)
1166 if (prediction_means[i][0] <
1167 (1.5 + sensor_max_range + max_vehicle_loc_uncertainty +
1169 fabs(prediction_means[i][1]) <
1170 (
DEG2RAD(20) + 0.5 * fov_yaw + max_vehicle_ang_uncertainty +
1174 out_LM_indices_to_predict.push_back(i);
1196 out_veh_increments[i] = 1e-6;
1198 out_feat_increments[i] = 1e-6;
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
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...
size_t getNumberOfLandmarksInTheMap() const
@ metricMaha
Mahalanobis distance.
Information for data-association:
void clear()
Clear the contents of the bi-map.
mrpt::math::TPose2D asTPose() const
static size_t get_vehicle_size()
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...
TOptions()
Default values.
bool create_simplemap
Whether to fill m_SFs (default=false)
const Scalar * const_iterator
const_iterator end() const
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.
int void fclose(FILE *f)
An OS-independent version of fclose.
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.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
GLdouble GLdouble GLdouble GLdouble q
std::shared_ptr< CSensoryFrame > Ptr
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
TDataAssocInfo m_last_data_association
Last data association.
#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...
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,...
static size_t get_observation_size()
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
static size_t get_feature_size()
KFVector m_xkk
The system state vector.
std::shared_ptr< CPosePDFGaussian > Ptr
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...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
float quantiles_3D_representation
Default = 3.
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Represents a probabilistic 3D (6D) movement.
uint64_t TLandmarkID
Unique IDs for landmarks.
std::shared_ptr< CObservationBearingRange > Ptr
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
std::vector< size_t > predictions_IDs
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
GLubyte GLubyte GLubyte GLubyte w
#define THROW_EXCEPTION(msg)
TDataAssociationMethod data_assoc_method
#define ASSERT_(f)
Defines an assertion mechanism.
T square(const T x)
Inline function for the square of a number.
std::shared_ptr< CActionRobotMovement2D > Ptr
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double RAD2DEG(const double x)
Radians to degrees.
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
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,...
CRangeBearingKFSLAM2D()
Default constructor.
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...
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,...
This namespace contains representation of robot actions and observations.
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
GLsizei GLsizei GLchar * source
CPose2D mean
The mean value.
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.
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.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
vector_KFArray_OBS all_predictions
KFMatrix m_pkk
The system full covariance matrix.
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,...
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
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.
This class allows loading and storing values and vectors of different types from a configuration text...
TDataAssociationMetric data_assoc_metric
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian &out_robotPose) const
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
TOptions options
The options for the algorithm.
double x() const
Common members of all points & poses classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
size_t size() const
Returns the count of pairs (pose,sensory data)
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
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'.
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
CPoint2D mean
The mean value.
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
This observation represents a number of range-bearing value pairs, each one for a detected landmark,...
A gaussian distribution for 2D points.
virtual ~CRangeBearingKFSLAM2D()
Destructor.
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
std::shared_ptr< CSetOfObjects > Ptr
double kftype
The numeric type used in the Kalman Filter (default=double)
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
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,...
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
const_iterator find_key(const KEY &k) const
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
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::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
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...
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...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
std::string 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...
GLenum GLenum GLvoid * row
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)
This is called between the KF prediction step and the update step, and the application must return th...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
std::shared_ptr< CActionRobotMovement3D > Ptr
This base provides a set of functions for maths stuff.
std::shared_ptr< CPose3DPDF > Ptr
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...
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
@ assocNN
Nearest-neighbor.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
mrpt::io::CFileOutputStream CFileOutputStream
GLsizei const GLchar ** string
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
std::shared_ptr< CEllipsoid > Ptr
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
TDataAssociationResults results
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
A class used to store a 3D point.
void clear()
Remove all stored pairs.
float std_sensor_range
The std.
std::shared_ptr< CActionCollection > Ptr
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
CPose3D mean
The mean value.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
double DEG2RAD(const double x)
Degrees to radians.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |