Go to the documentation of this file.
31 #include <Eigen/Dense>
137 std::vector<TPoint3D>& out_landmarksPositions,
138 std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
160 out_landmarksPositions.resize(nLMs);
161 for (i = 0; i < nLMs; i++)
163 out_landmarksPositions[i].x =
165 out_landmarksPositions[i].y =
167 out_landmarksPositions[i].z =
178 out_fullCovariance =
m_pkk;
227 vector<std::vector<uint32_t>> partitions;
234 vector<std::vector<uint32_t>> partitions;
235 std::vector<uint32_t> tmpCluster;
240 for (
size_t i = 0; i <
m_SFs.
size(); i++)
242 tmpCluster.push_back(i);
245 partitions.push_back(tmpCluster);
247 tmpCluster.push_back(i);
267 m_action->getBestMovementEstimation();
277 actMov2D->poseChange->getMean(estMovMean);
295 i < static_cast<KFArray_ACT::Index>(u.size()); i++)
303 const KFArray_ACT& u, KFArray_VEH& xv,
bool& out_skipPrediction)
const
313 out_skipPrediction =
true;
324 odoIncrement.
m_quat[0] = u[3];
325 odoIncrement.
m_quat[1] = u[4];
326 odoIncrement.
m_quat[2] = u[5];
327 odoIncrement.
m_quat[3] = u[6];
330 robotPose += odoIncrement;
333 for (
size_t i = 0; i < xv.SizeAtCompileTime; i++) xv[i] = robotPose[i];
354 CPose3DQuatPDF::jacobiansPoseComposition(
395 odoIncr2D.
copyFrom(*act2D->poseChange);
419 const std::vector<size_t>& idx_landmarks_to_predict,
420 vector_KFArray_OBS& out_predictions)
const
432 "*ERROR*: This method requires an observation of type "
433 "CObservationBearingRange");
463 const CPose3DQuat sensorPoseAbs = robotPose + sensorPoseOnRobot;
465 const size_t N = idx_landmarks_to_predict.size();
466 out_predictions.resize(N);
467 for (
size_t i = 0; i < N; i++)
469 const size_t row_in = feature_size * idx_landmarks_to_predict[i];
473 m_xkk[vehicle_size + row_in + 0],
m_xkk[vehicle_size + row_in + 1],
474 m_xkk[vehicle_size + row_in + 2]);
480 out_predictions[i][0],
481 out_predictions[i][1],
482 out_predictions[i][2]
490 size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy)
const
502 "*ERROR*: This method requires an observation of type "
503 "CObservationBearingRange");
518 CPose3DQuatPDF::jacobiansPoseComposition(
519 robotPose, sensorPoseOnRobot, H_senpose_vehpose, H_senpose_senrelpose,
522 const size_t row_in = feature_size * idx_landmark_to_predict;
526 m_xkk[vehicle_size + row_in + 0],
m_xkk[vehicle_size + row_in + 1],
527 m_xkk[vehicle_size + row_in + 2]);
540 Hx = Hx_sensor * H_senpose_vehpose;
568 vector_KFArray_OBS& Z, std::vector<int>& data_association,
569 const vector_KFArray_OBS& all_predictions,
const KFMatrix& S,
570 const std::vector<size_t>& lm_indices_in_S,
const KFMatrix_OxO&
R)
578 CObservationBearingRange::TMeasurementList::const_iterator itObs;
584 "*ERROR*: This method requires an observation of type "
585 "CObservationBearingRange");
587 const size_t N = obs->sensedData.size();
591 for (row = 0, itObs = obs->sensedData.begin();
592 itObs != obs->sensedData.end(); ++itObs, ++row)
595 Z[row][0] = itObs->range;
596 Z[row][1] = itObs->yaw;
597 Z[row][2] = itObs->pitch;
602 data_association.assign(N, -1);
605 std::vector<size_t> obs_idxs_needing_data_assoc;
606 obs_idxs_needing_data_assoc.reserve(N);
609 std::vector<int>::iterator itDA;
610 for (row = 0, itObs = obs->sensedData.begin(),
611 itDA = data_association.begin();
612 itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
615 if (itObs->landmarkID < 0)
616 obs_idxs_needing_data_assoc.push_back(row);
622 *itDA = itID->second;
631 if (obs_idxs_needing_data_assoc.empty())
637 for (
size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
639 int da = data_association[idxObs];
642 data_association[idxObs];
648 const size_t nObsDA = obs_idxs_needing_data_assoc.size();
651 for (
size_t i = 0; i < nObsDA; i++)
653 const size_t idx = obs_idxs_needing_data_assoc[i];
654 for (
unsigned k = 0; k < obs_size; k++)
655 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];
700 data_association[it->first] = it->second;
716 const double T = std::sqrt(
719 ASSERTMSG_(T > 0,
"Vehicle pose quaternion norm is not >0!!");
721 const double T_ = (
m_xkk[3] < 0 ? -1.0 : 1.0) / T;
787 std_sensor_pitch(
DEG2RAD(0.2f))
802 out <<
"\n----------- [CRangeBearingKFSLAM::TOptions] ------------ \n\n";
805 "doPartitioningExperiment = %c\n",
806 doPartitioningExperiment ?
'Y' :
'N');
808 "partitioningMethod = %i\n", partitioningMethod);
810 "data_assoc_method = %s\n",
814 "data_assoc_metric = %s\n",
818 "data_assoc_IC_chi2_thres = %.06f\n",
819 data_assoc_IC_chi2_thres);
821 "data_assoc_IC_metric = %s\n",
825 "data_assoc_IC_ml_threshold = %.06f\n",
826 data_assoc_IC_ml_threshold);
841 "*ERROR*: This method requires an observation of type "
842 "CObservationBearingRange");
855 CPose3DQuatPDF::jacobiansPoseComposition(
856 robotPose, sensorPoseOnRobot, dsensorabs_dvehpose,
857 dsensorabs_dsenrelpose, &sensorPoseAbs);
883 hn_r * chn_y * chn_p, hn_r * shn_y * chn_p, -hn_r * shn_p);
894 const kftype values_dynlocal_dhn[] = {chn_p * chn_y,
895 -hn_r * chn_p * shn_y,
896 -hn_r * chn_y * shn_p,
898 hn_r * chn_p * chn_y,
899 -hn_r * shn_p * shn_y,
909 yn_rel_sensor.
x, yn_rel_sensor.
y,
912 &jacob_dyn_dynrelsensor, &jacob_dyn_dsensorabs);
914 dyn_dhn = jacob_dyn_dynrelsensor * dynlocal_dhn;
917 dyn_dxv = jacob_dyn_dsensorabs * dsensorabs_dvehpose;
933 const size_t in_obsIdx,
const size_t in_idxNewFeat)
941 "*ERROR*: This method requires an observation of type "
942 "CObservationBearingRange");
947 ASSERT_(in_obsIdx < obs->sensedData.size());
948 if (obs->sensedData[in_obsIdx].landmarkID >= 0)
951 m_IDs.
insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
985 ellip->setPose(pointGauss.
mean);
986 ellip->setCovMatrix(pointGauss.
cov);
987 ellip->enableDrawSolid3D(
false);
989 ellip->set3DsegmentsCount(10);
990 ellip->setColor(1, 0, 0);
992 outObj->insert(ellip);
997 for (
size_t i = 0; i < nLMs; i++)
1013 ellip->setName(
format(
"%u",
static_cast<unsigned int>(i)));
1014 ellip->enableShowName(
true);
1015 ellip->setPose(pointGauss.
mean);
1016 ellip->setCovMatrix(pointGauss.
cov);
1017 ellip->enableDrawSolid3D(
false);
1019 ellip->set3DsegmentsCount(10);
1021 ellip->setColor(0, 0, 1);
1027 map<int, bool> belongToPartition;
1047 for (
auto& o : obs->sensedData)
1049 if (o.landmarkID == i_th_ID)
1051 belongToPartition[p] =
true;
1059 string strParts(
"[");
1061 for (
auto it = belongToPartition.begin();
1062 it != belongToPartition.end(); ++it)
1064 if (it != belongToPartition.begin()) strParts += string(
",");
1065 strParts +=
format(
"%i", it->first);
1068 ellip->setName(ellip->getName() + strParts +
string(
"]"));
1071 outObj->insert(ellip);
1079 size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership)
1087 vector<std::vector<uint32_t>> partitions;
1088 std::vector<uint32_t> tmpCluster;
1090 for (
size_t i = 0; i <
m_SFs.
size(); i++)
1092 tmpCluster.push_back(i);
1095 partitions.push_back(tmpCluster);
1097 tmpCluster.push_back(
1114 std::vector<std::vector<uint32_t>>& landmarksMembership)
const
1116 landmarksMembership.clear();
1123 for (
size_t i = 0; i < nLMs; i++)
1125 map<int, bool> belongToPartition;
1145 for (
auto& o : obs->sensedData)
1147 if (o.landmarkID == i_th_ID)
1149 belongToPartition[p] =
true;
1157 std::vector<uint32_t> membershipOfThisLM;
1159 for (
auto& it : belongToPartition)
1160 membershipOfThisLM.push_back(it.first);
1162 landmarksMembership.push_back(membershipOfThisLM);
1170 const std::vector<std::vector<uint32_t>>& landmarksMembership)
const
1178 fullCov(i, i) = max(fullCov(i, i), 1e-6);
1183 double sumOffBlocks = 0;
1184 unsigned int nLMs = landmarksMembership.size();
1189 for (i = 0; i < nLMs; i++)
1191 for (
size_t j = i + 1; j < nLMs; j++)
1196 landmarksMembership[i], landmarksMembership[j]))
1200 sumOffBlocks += 2 * H.
block<2, 2>(row, col).
sum();
1205 return sumOffBlocks / H.
asEigen()
1221 vector<std::vector<uint32_t>> partitions;
1230 const string& fil,
float stdCount,
const string& styleLandmarks,
1231 const string& stylePath,
const string& styleRobot)
const
1242 "%%--------------------------------------------------------------------"
1244 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1248 "'CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile'\n");
1252 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1256 "%%--------------------------------------------------------------------"
1264 for (
size_t i = 0; i < nLMs; i++)
1268 cov(0, 0) =
m_pkk(idx + 0, idx + 0);
1269 cov(1, 1) =
m_pkk(idx + 1, idx + 1);
1287 for (
size_t i = 0; i <
m_SFs.
size(); i++)
1302 f,
"plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1361 std::vector<size_t>& out_LM_indices_to_predict)
const
1367 "*ERROR*: This method requires an observation of type "
1368 "CObservationBearingRange");
1370 #define USE_HEURISTIC_PREDICTION
1372 #ifdef USE_HEURISTIC_PREDICTION
1373 const double sensor_max_range = obs->maxSensorDistance;
1374 const double fov_yaw = obs->fieldOfView_yaw;
1375 const double fov_pitch = obs->fieldOfView_pitch;
1377 const double max_vehicle_loc_uncertainty =
1381 out_LM_indices_to_predict.clear();
1382 for (
size_t i = 0; i < prediction_means.size(); i++)
1384 #ifndef USE_HEURISTIC_PREDICTION
1385 out_LM_indices_to_predict.push_back(i);
1388 if (prediction_means[i][0] <
1389 (15 + sensor_max_range + max_vehicle_loc_uncertainty +
1391 fabs(prediction_means[i][1]) <
1393 fabs(prediction_means[i][2]) <
1396 out_LM_indices_to_predict.push_back(i);
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
mrpt::math::CQuaternionDouble m_quat
The quaternion.
mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
size_t getNumberOfLandmarksInTheMap() const
float std_sensor_range
The std.
static constexpr size_t get_vehicle_size()
void OnGetObservationNoise(KFMatrix_OxO &out_R) const override
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
void clear()
Clear the contents of the bi-map.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
void assign(const std::size_t N, const Scalar value)
double computeOffDiagonalBlocksApproximationError(const std::vector< std::vector< uint32_t >> &landmarksMembership) const
Computes the ratio of the missing information matrix elements which are ignored under a certain parti...
void OnTransitionJacobian(KFMatrix_VxV &out_F) const override
Implements the transition Jacobian .
const_iterator end() const
int void fclose(FILE *f)
An OS-independent version of fclose.
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
bool create_simplemap
Whether to fill m_SFs (default=false)
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
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...
TDataAssociationMethod data_assoc_method
void OnGetAction(KFArray_ACT &out_u) const override
Must return the action vector u.
#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...
~CRangeBearingKFSLAM() override
Destructor:
mrpt::math::CVectorFixed< double, FEAT_SIZE > KFArray_FEAT
CIncrementalMapPartitioner mapPartitioner
Used for map partitioning experiments.
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
int64_t TLandmarkID
The type for the IDs of landmarks.
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...
KFVector m_xkk
The system state vector.
TDataAssociationResults results
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.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
A gaussian distribution for 3D points.
double x() const
Common members of all points & poses classes.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
double mean(const CONTAINER &v)
Computes the mean value of a vector.
mrpt::math::CMatrixFixed< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
std::vector< std::vector< uint32_t > > m_lastPartitionSet
Represents a probabilistic 3D (6D) movement.
uint64_t TLandmarkID
Unique IDs for landmarks.
std::shared_ptr< mrpt::obs ::CObservationBearingRange > Ptr
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
static constexpr size_t get_feature_size()
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
mrpt::vision::TStereoCalibResults out
void getCurrentState(mrpt::poses::CPose3DQuatPDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint3D > &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.
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
void getLastPartitionLandmarksAsIfFixedSubmaps(size_t K, std::vector< std::vector< uint32_t >> &landmarksMembership)
For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size subm...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
std::shared_ptr< mrpt::obs ::CActionRobotMovement3D > Ptr
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
#define THROW_EXCEPTION(msg)
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
Implements the transition model .
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
void OnNormalizeStateVector() override
This method is called after the prediction and after the update, to give the user an opportunity to n...
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,...
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
void resize(std::size_t N, bool zeroNewElements=false)
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...
This namespace contains representation of robot actions and observations.
mrpt::poses::CPose3DQuat getIncrementFromOdometry() const
Return the last odometry, as a pose increment.
mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const
Get the current robot pose mean, as a 3D+quaternion pose.
void OnTransitionNoise(KFMatrix_VxV &out_Q) const override
Implements the transition noise covariance .
std::shared_ptr< mrpt::poses ::CPose3DPDFGaussian > Ptr
TOptions options
Algorithm parameters.
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.
std::vector< KFArray_OBS > vector_KFArray_OBS
void OnObservationJacobians(size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .
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.
KFMatrix m_pkk
The system full covariance matrix.
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
TDataAssocInfo m_last_data_association
Last data association.
A compile-time fixed-size numeric matrix container.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::CVectorFixed< double, OBS_SIZE > KFArray_OBS
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
constexpr double RAD2DEG(const double x)
Radians to degrees.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
CRangeBearingKFSLAM()
Constructor.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
mrpt::math::CMatrixDynamic< kftype > Y_pred_means
mrpt::math::CMatrixDynamic< kftype > Y_pred_covs
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
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...
size_t size() const
Returns the count of pairs (pose,sensory data)
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
TDataAssociationMetric data_assoc_metric
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'.
float std_odo_z_additional
Additional std.
bool inverse(const VALUE &v, KEY &out_k) const
Get the key associated the given value, VALUE->KEY, returning false if not present.
Information for data-association:
size_t countCommonElements(const CONTAINER1 &a, const CONTAINER2 &b)
Counts the number of elements that appear in both STL-like containers (comparison through the == oper...
This observation represents a number of range-bearing value pairs, each one for a detected landmark,...
return_t square(const num_t x)
Inline function for the square of a number.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacob_dryp_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacob_dryp_dpose=nullptr) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
void reconsiderPartitionsNow()
The partitioning of the entire map is recomputed again.
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...
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...
constexpr double DEG2RAD(const double x)
Degrees to radians
double kftype
The numeric type used in the Kalman Filter (default=double)
void getCurrentRobotPose(mrpt::poses::CPose3DQuatPDFGaussian &out_robotPose) const
Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion).
mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
const_iterator find_key(const KEY &k) const
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
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.
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,...
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
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
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...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
CPose3DQuat mean
The mean value.
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...
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
bool force_ignore_odometry
Whether to ignore the input odometry and behave as if there was no odometry at all (default: false)
This base provides a set of functions for maths stuff.
std::shared_ptr< CPose3DPDF > Ptr
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
mrpt::slam::CRangeBearingKFSLAM::TOptions options
#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::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
size_type cols() const
Number of columns in the matrix.
mrpt::math::CVectorFloat stds_Q_no_odo
A 7-length vector with the std.
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
std::vector< size_t > predictions_IDs
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
static Ptr Create(Args &&... args)
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z].
int partitioningMethod
Applicable only if "doPartitioningExperiment=true".
void clear()
Remove all stored pairs.
TOptions()
Default values.
CPoint3D mean
The mean value.
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
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 ....
@ UNINITIALIZED_QUATERNION
static constexpr size_t get_observation_size()
float quantiles_3D_representation
Default = 3.
size_type rows() const
Number of rows in the matrix.
std::string std::string format(std::string_view fmt, ARGS &&... args)
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...
bool doPartitioningExperiment
If set to true (default=false), map will be partitioned using the method stated by partitioningMethod...
void getLastPartitionLandmarks(std::vector< std::vector< uint32_t >> &landmarksMembership) const
Return the partitioning of the landmarks in clusters accoring to the last partition.
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
Implements the observation prediction .
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020 | |