36 #define STATS_EXPERIMENT 0 37 #define STATS_EXPERIMENT_ALSO_NC 1 43 : options(), m_action(), m_SF(), m_IDs(), m_SFs()
85 m_pkk.extractMatrix(0, 0, COV);
86 out_robotPose.
cov = COV;
96 std::vector<TPoint2D>& out_landmarksPositions,
97 std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
109 m_pkk.extractMatrix(0, 0, COV);
110 out_robotPose.
cov = COV;
114 size_t i, nLMs = (
m_xkk.size() - 3) / 2;
115 out_landmarksPositions.resize(nLMs);
116 for (i = 0; i < nLMs; i++)
118 out_landmarksPositions[i].x =
m_xkk[3 + i * 2 + 0];
119 out_landmarksPositions[i].y =
m_xkk[3 + i * 2 + 1];
126 out_fullState.resize(
m_xkk.size());
127 for (KFVector::Index i = 0; i <
m_xkk.size(); i++)
128 out_fullState[i] =
m_xkk[i];
131 out_fullCovariance =
m_pkk;
162 mrpt::make_aligned_shared<CPosePDFGaussian>();
177 m_action->getBestMovementEstimation();
183 u[1] = actMov3D->poseChange.mean.y();
184 u[2] = actMov3D->poseChange.mean.yaw();
189 actMov2D->poseChange->getMean(estMovMean);
190 u[0] = estMovMean.
x();
191 u[1] = estMovMean.
y();
192 u[2] = estMovMean.
phi();
197 for (
size_t i = 0; i < 3; i++) u[i] = 0;
205 const KFArray_ACT& u, KFArray_VEH& xv,
bool& out_skipPrediction)
const 215 out_skipPrediction =
true;
219 CPose2D robotPose(xv[0], xv[1], xv[2]);
220 CPose2D odoIncrement(u[0], u[1], u[2]);
223 robotPose = robotPose + odoIncrement;
225 xv[0] = robotPose.
x();
226 xv[1] = robotPose.
y();
227 xv[2] = robotPose.
phi();
244 if (act3D && act2D)
THROW_EXCEPTION(
"Both 2D & 3D odometry are present!?!?")
254 Ap =
TPoint2D(act2D->poseChange->getMeanVal());
271 F.get_unsafe(0, 2) = -Ax * sy - Ay * cy;
272 F.get_unsafe(1, 2) = Ax * cy - Ay * sy;
289 if (act3D && act2D)
THROW_EXCEPTION(
"Both 2D & 3D odometry are present!?!?")
293 if (!act3D && !act2D)
299 for (
size_t i = 0; i < 3; i++)
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,
vector_int& data_association,
528 const vector_KFArray_OBS& all_predictions,
const KFMatrix& S,
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);
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)
799 "\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n");
802 "data_assoc_method = %s\n",
806 "data_assoc_metric = %s\n",
810 "data_assoc_IC_chi2_thres = %.06f\n",
811 data_assoc_IC_chi2_thres);
813 "data_assoc_IC_metric = %s\n",
817 "data_assoc_IC_ml_threshold = %.06f\n",
818 data_assoc_IC_ml_threshold);
853 "*ERROR*: This method requires an observation of type " 854 "CObservationBearingRange")
855 const CPose2D sensorPoseOnRobot =
CPose2D(obs->sensorLocationOnRobot);
865 const kftype cphi0 = cos(phi0);
866 const kftype sphi0 = sin(phi0);
869 const kftype x0s = sensorPoseOnRobot.x();
870 const kftype y0s = sensorPoseOnRobot.y();
871 const kftype phis = sensorPoseOnRobot.phi();
873 const kftype hr = in_z[0];
874 const kftype ha = in_z[1];
876 const kftype cphi_0sa = cos(phi0 + phis + ha);
877 const kftype sphi_0sa = sin(phi0 + phis + ha);
880 yn[0] = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s + x0;
881 yn[1] = hr * sphi_0sa + sphi0 * x0s + cphi0 * y0s + y0;
884 dyn_dxv.get_unsafe(0, 0) = 1;
885 dyn_dxv.get_unsafe(0, 1) = 0;
886 dyn_dxv.get_unsafe(0, 2) = -hr * sphi_0sa - sphi0 * x0s - cphi0 * y0s;
888 dyn_dxv.get_unsafe(1, 0) = 0;
889 dyn_dxv.get_unsafe(1, 1) = 1;
890 dyn_dxv.get_unsafe(1, 2) = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s;
893 dyn_dhn.get_unsafe(0, 0) = cphi_0sa;
894 dyn_dhn.get_unsafe(0, 1) = -hr * sphi_0sa;
896 dyn_dhn.get_unsafe(1, 0) = sphi_0sa;
897 dyn_dhn.get_unsafe(1, 1) = hr * cphi_0sa;
913 const size_t in_obsIdx,
const size_t in_idxNewFeat)
921 "*ERROR*: This method requires an observation of type " 922 "CObservationBearingRange")
927 ASSERT_(in_obsIdx < obs->sensedData.size())
929 if (obs->sensedData[in_obsIdx].landmarkID >= 0)
932 m_IDs.
insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
964 m_pkk.extractMatrix(0, 0, 2, 2, COV);
965 pointGauss.
cov = COV;
969 mrpt::make_aligned_shared<opengl::CEllipsoid>();
971 ellip->setPose(pointGauss.
mean);
972 ellip->setCovMatrix(pointGauss.
cov);
973 ellip->enableDrawSolid3D(
false);
975 ellip->set3DsegmentsCount(10);
976 ellip->setColor(1, 0, 0);
978 outObj->insert(ellip);
982 const size_t nLMs = (
m_xkk.size() - 3) / 2;
983 for (
size_t i = 0; i < nLMs; i++)
987 m_pkk.extractMatrix(3 + 2 * i, 3 + 2 * i, 2, 2, COV);
988 pointGauss.
cov = COV;
991 mrpt::make_aligned_shared<opengl::CEllipsoid>();
993 ellip->setName(
format(
"%u", static_cast<unsigned int>(i)));
994 ellip->enableShowName(
true);
995 ellip->setPose(pointGauss.
mean);
996 ellip->setCovMatrix(pointGauss.
cov);
997 ellip->enableDrawSolid3D(
false);
999 ellip->set3DsegmentsCount(10);
1001 ellip->setColor(0, 0, 1);
1003 outObj->insert(ellip);
1011 const string& fil,
float stdCount,
const string& styleLandmarks,
1012 const string& stylePath,
const string& styleRobot)
const 1023 "%%--------------------------------------------------------------------" 1025 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1029 "'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'" 1034 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1038 "%%--------------------------------------------------------------------" 1046 for (i = 0; i < nLMs; i++)
1050 cov(0, 0) =
m_pkk(idx + 0, idx + 0);
1051 cov(1, 1) =
m_pkk(idx + 1, idx + 1);
1085 f,
"plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1149 "*ERROR*: This method requires an observation of type " 1150 "CObservationBearingRange")
1152 const double sensor_max_range = obs->maxSensorDistance;
1153 const double fov_yaw = obs->fieldOfView_yaw;
1155 const double max_vehicle_loc_uncertainty =
1156 4 * std::sqrt(
m_pkk.get_unsafe(0, 0) +
m_pkk.get_unsafe(1, 1));
1157 const double max_vehicle_ang_uncertainty =
1158 4 * std::sqrt(
m_pkk.get_unsafe(2, 2));
1160 out_LM_indices_to_predict.clear();
1161 for (
size_t i = 0; i < prediction_means.size(); i++)
1165 if (prediction_means[i][0] <
1166 (1.5 + sensor_max_range + max_vehicle_loc_uncertainty +
1168 fabs(prediction_means[i][1]) <
1169 (
DEG2RAD(20) + 0.5 * fov_yaw + max_vehicle_ang_uncertainty +
1173 out_LM_indices_to_predict.push_back(i);
1195 out_veh_increments[i] = 1e-6;
1197 out_feat_increments[i] = 1e-6;
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
CPoint2D mean
The mean value.
double x() const
Common members of all points & poses classes.
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
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...
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.
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.
int void fclose(FILE *f)
An OS-independent version of fclose.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
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...
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
const Scalar * const_iterator
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
KFMatrix m_pkk
The system full covariance matrix.
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
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 ...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
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)
mrpt::utils::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
Represents a probabilistic 3D (6D) movement.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
void clear()
Clear the contents of the bi-map.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
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 ...
std::shared_ptr< CActionRobotMovement2D > Ptr
static size_t get_vehicle_size()
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
This CStream derived class allow using a file as a write-only, binary stream.
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).
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
static size_t get_observation_size()
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...
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)
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
const_iterator find_key(const KEY &k) const
std::shared_ptr< CPose3DPDF > Ptr
std::shared_ptr< CSetOfObjects > Ptr
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
std::shared_ptr< CSensoryFrame > Ptr
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
std::shared_ptr< CPosePDFGaussian > Ptr
std::shared_ptr< CActionRobotMovement3D > Ptr
size_t size() const
Returns the count of pairs (pose,sensory data)
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'.
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...
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.
double kftype
The numeric type used in the Kalman Filter (default=double)
CRangeBearingKFSLAM2D()
Default constructor.
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
uint64_t TLandmarkID
Unique IDs for landmarks.
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.
std::shared_ptr< CActionCollection > Ptr
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_range
The std.
std::shared_ptr< CObservationBearingRange > Ptr
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
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.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
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...
GLsizei GLsizei GLchar * source
void OnObservationModel(const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
std::vector< size_t > vector_size_t
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
std::vector< int32_t > vector_int
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
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...
TOptions()
Default values.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
#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...
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...
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
#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...
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.
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.
std::shared_ptr< CEllipsoid > Ptr
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.