30 #include <Eigen/Dense> 49 CLandmarksMap::TMapDefinition::TMapDefinition() =
default;
50 void CLandmarksMap::TMapDefinition::loadFromConfigFile_map_specific(
56 sectionNamePrefix +
string(
"_creationOpts");
57 this->initialBeacons.clear();
58 const unsigned int nBeacons =
source.read_int(sSectCreation,
"nBeacons", 0);
59 for (
unsigned int q = 1;
q <= nBeacons;
q++)
63 source.read_int(sSectCreation,
format(
"beacon_%03u_ID",
q), 0);
66 source.read_float(sSectCreation,
format(
"beacon_%03u_x",
q), 0);
68 source.read_float(sSectCreation,
format(
"beacon_%03u_y",
q), 0);
70 source.read_float(sSectCreation,
format(
"beacon_%03u_z",
q), 0);
72 this->initialBeacons.push_back(newPair);
75 insertionOpts.loadFromConfigFile(
76 source, sectionNamePrefix +
string(
"_insertOpts"));
77 likelihoodOpts.loadFromConfigFile(
78 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
81 void CLandmarksMap::TMapDefinition::dumpToTextStream_map_specific(
82 std::ostream& out)
const 85 "number of initial beacons = %u\n",
86 (
int)initialBeacons.size());
90 "--------------------------------------------------------\n");
91 for (
const auto& initialBeacon : initialBeacons)
93 " %03u (%8.03f,%8.03f,%8.03f)\n", initialBeacon.second,
94 initialBeacon.first.x, initialBeacon.first.y,
95 initialBeacon.first.z);
97 this->insertionOpts.dumpToTextStream(out);
98 this->likelihoodOpts.dumpToTextStream(out);
115 lm.
features[0].keypoint.ID = initialBeacon.second;
116 lm.
ID = initialBeacon.second;
123 obj->landmarks.push_back(lm);
141 CLandmarksMap::_mEDD;
143 bool CLandmarksMap::_maxIDUpdated =
false;
145 void CLandmarksMap::internal_clear() { landmarks.clear(); }
149 uint8_t CLandmarksMap::serializeGetVersion()
const {
return 0; }
158 for (
const auto& landmark : landmarks) out << landmark;
161 void CLandmarksMap::serializeFrom(
182 for (i = 0; i <
n; i++)
185 landmarks.push_back(lm);
197 double CLandmarksMap::internal_computeObservationLikelihood(
203 insertionOptions.insert_Landmarks_from_range_scans)
210 CPose2D sensorPose2D(robotPose3D + o.sensorPose);
213 o, &robotPose3D, likelihoodOptions.rangeScan2D_decimation);
216 return computeLikelihood_RSLC_2007(&auxMap, sensorPose2D);
229 o, CLandmarksMap::_mapMaxID, likelihoodOptions.SIFT_feat_options);
235 if (!CLandmarksMap::_maxIDUpdated)
237 CLandmarksMap::_mapMaxID += auxMap.
size();
238 CLandmarksMap::_maxIDUpdated =
true;
242 return computeLikelihood_SIFT_LandmarkMap(&auxMap);
256 std::deque<CObservationBeaconRanges::TMeasurement>::const_iterator it;
257 TSequenceLandmarks::iterator lm_it;
262 for (it = o.sensedData.begin(); it != o.sensedData.end(); it++)
265 unsigned int sensedID = it->beaconID;
268 for (lm_it = landmarks.begin(); !found && lm_it != landmarks.end();
272 (lm_it->ID == sensedID) &&
273 (!std::isnan(it->sensedDistance)))
275 lm_it->getPose(beaconPDF);
276 beacon3D = beaconPDF.
mean;
279 point3D = robotPose3D + it->sensorLocationOnRobot;
281 const float expectedRange = point3D.
distanceTo(beacon3D);
282 float sensedDist = it->sensedDistance;
283 if (sensedDist < 0) sensedDist = 0;
286 likelihoodOptions.beaconRangesUseObservationStd
288 : likelihoodOptions.beaconRangesStd;
291 square((expectedRange - sensedDist) / sensorStd));
299 if (o.maxSensorDistance != o.minSensorDistance)
301 log(1.0 / (o.maxSensorDistance - o.minSensorDistance));
322 CPose3D sensorPose3D = robotPose3D + o.sensorPose;
326 CMatrixD dij(1, 6), Cij(6, 6), Cij_1;
327 dij(0, 0) = o.pose.mean.x() - sensorPose3D.
x();
328 dij(0, 1) = o.pose.mean.y() - sensorPose3D.
y();
329 dij(0, 2) = o.pose.mean.z() - sensorPose3D.z();
330 dij(0, 3) =
wrapToPi(o.pose.mean.yaw() - sensorPose3D.
yaw());
331 dij(0, 4) =
wrapToPi(o.pose.mean.pitch() - sensorPose3D.
pitch());
332 dij(0, 5) =
wrapToPi(o.pose.mean.roll() - sensorPose3D.
roll());
340 -0.5 * (distMahaFlik2 /
square(likelihoodOptions.extRobotPoseStd));
358 double earth_radius = 6378137;
360 if ((o.has_GGA_datum) &&
361 (likelihoodOptions.GPSOrigin.min_sat <=
369 likelihoodOptions.GPSOrigin.longitude)) *
374 likelihoodOptions.GPSOrigin.latitude)) *
377 (
x * cos(likelihoodOptions.GPSOrigin.ang) +
378 y * sin(likelihoodOptions.GPSOrigin.ang) +
379 likelihoodOptions.GPSOrigin.x_shift));
381 (-
x * sin(likelihoodOptions.GPSOrigin.ang) +
382 y * cos(likelihoodOptions.GPSOrigin.ang) +
383 likelihoodOptions.GPSOrigin.y_shift));
387 likelihoodOptions.GPSOrigin.altitude));
438 bool CLandmarksMap::internal_insertObservation(
448 robotPose2D =
CPose2D(*robotPose);
449 robotPose3D = (*robotPose);
457 insertionOptions.insert_SIFTs_from_monocular_images)
469 o, insertionOptions.SIFT_feat_options);
506 insertionOptions.insert_SIFTs_from_stereo_images)
517 o, CLandmarksMap::_mapMaxID, insertionOptions.SIFT_feat_options);
537 CPose3D acumTransform(robotPose3D + o.refCameraPose);
541 fuseWith(auxMap,
true);
561 void CLandmarksMap::computeMatchingWith2D(
563 float maxDistForCorrespondence,
float maxAngularDistForCorrespondence,
565 float& correspondencesRatio,
float* sumSqrDist,
bool onlyKeepTheClosest,
566 bool onlyUniqueRobust)
const 578 CPose3D otherMapPose3D(otherMapPose);
580 correspondencesRatio = 0;
584 const auto* otherMap2 =
static_cast<const CLandmarksMap*
>(otherMap);
585 std::vector<bool> otherCorrespondences;
591 computeMatchingWith3DLandmarks(
592 otherMap2, correspondences, correspondencesRatio, otherCorrespondences);
600 void CLandmarksMap::loadSiftFeaturesFromImageObservation(
606 float d = insertionOptions.SIFTsLoadDistanceOfTheMean;
607 float width = insertionOptions.SIFTsLoadEllipsoidWidth;
632 for (sift = siftList.
begin(); sift != siftList.
end(); sift++)
643 landmark3DPositionPDF.
mean *= d;
649 D(0, 0) =
square(0.5 * d);
662 Normal *= -1 / Normal.
norm();
675 landmarks.push_back(lm);
683 void CLandmarksMap::loadSiftFeaturesFromStereoImageObservation(
698 fExt.options = feat_options;
705 insertionOptions.SIFTs_numberOfKLTKeypoints);
708 insertionOptions.SIFTs_numberOfKLTKeypoints);
712 matchingOptions.
epipolar_TH = insertionOptions.SIFTs_epipolar_TH;
713 matchingOptions.
EDD_RATIO = insertionOptions.SiftCorrRatioThreshold;
714 matchingOptions.
maxEDD_TH = insertionOptions.SiftEDDThreshold;
716 leftSiftList, rightSiftList, matchesList, matchingOptions);
718 if (insertionOptions.PLOT_IMAGES)
720 std::cerr <<
"Warning: insertionOptions.PLOT_IMAGES has no effect " 721 "since MRPT 0.9.1\n";
736 stereoParams.
stdPixel = insertionOptions.SIFTs_stdXY;
737 stereoParams.
stdDisp = insertionOptions.SIFTs_stdDisparity;
739 stereoParams.
minZ = 0.0f;
740 stereoParams.
maxZ = insertionOptions.SIFTs_stereo_maxDepth;
742 size_t numM = matchesList.size();
747 for (ii = landmarks.begin(); ii != landmarks.end(); ii++)
750 (*ii).seenTimesCount = 1;
754 "%u (out of %u) corrs!\n", static_cast<unsigned>(landmarks.size()),
755 static_cast<unsigned>(numM));
768 void CLandmarksMap::changeCoordinatesReference(
const CPose3D& newOrg)
770 TSequenceLandmarks::iterator lm;
776 double R11 = HM(0, 0);
777 double R12 = HM(0, 1);
778 double R13 = HM(0, 2);
779 double R21 = HM(1, 0);
780 double R22 = HM(1, 1);
781 double R23 = HM(1, 2);
782 double R31 = HM(2, 0);
783 double R32 = HM(2, 1);
784 double R33 = HM(2, 2);
786 double c11, c22, c33, c12, c13, c23;
790 for (lm = landmarks.begin(); lm != landmarks.end(); lm++)
796 float C11 = lm->pose_cov_11;
797 float C22 = lm->pose_cov_22;
798 float C33 = lm->pose_cov_33;
799 float C12 = lm->pose_cov_12;
800 float C13 = lm->pose_cov_13;
801 float C23 = lm->pose_cov_23;
804 c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
805 R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
806 R13 * (C13 * R11 + C23 * R12 + C33 * R13);
807 c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
808 (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
809 (C13 * R11 + C23 * R12 + C33 * R13) * R23;
810 c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
811 (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
812 (C13 * R11 + C23 * R12 + C33 * R13) * R33;
813 c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
814 R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
815 R23 * (C13 * R21 + C23 * R22 + C33 * R23);
816 c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
817 (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
818 (C13 * R21 + C23 * R22 + C33 * R23) * R33;
819 c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
820 R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
821 R33 * (C13 * R31 + C23 * R32 + C33 * R33);
824 lm->pose_cov_11 = c11;
825 lm->pose_cov_22 = c22;
826 lm->pose_cov_33 = c33;
827 lm->pose_cov_12 = c12;
828 lm->pose_cov_13 = c13;
829 lm->pose_cov_23 = c23;
833 float Nx = lm->normal.x;
834 float Ny = lm->normal.y;
835 float Nz = lm->normal.z;
837 lm->normal.x = Nx * R11 + Ny * R12 + Nz * R13;
838 lm->normal.y = Nx * R21 + Ny * R22 + Nz * R23;
839 lm->normal.z = Nx * R31 + Ny * R32 + Nz * R33;
843 landmarks.hasBeenModifiedAll();
849 void CLandmarksMap::changeCoordinatesReference(
852 TSequenceLandmarks::const_iterator lm;
859 double R11 = HM(0, 0);
860 double R12 = HM(0, 1);
861 double R13 = HM(0, 2);
862 double R21 = HM(1, 0);
863 double R22 = HM(1, 1);
864 double R23 = HM(1, 2);
865 double R31 = HM(2, 0);
866 double R32 = HM(2, 1);
867 double R33 = HM(2, 2);
869 double c11, c22, c33, c12, c13, c23;
883 float C11 = lm->pose_cov_11;
884 float C22 = lm->pose_cov_22;
885 float C33 = lm->pose_cov_33;
886 float C12 = lm->pose_cov_12;
887 float C13 = lm->pose_cov_13;
888 float C23 = lm->pose_cov_23;
891 c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
892 R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
893 R13 * (C13 * R11 + C23 * R12 + C33 * R13);
894 c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
895 (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
896 (C13 * R11 + C23 * R12 + C33 * R13) * R23;
897 c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
898 (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
899 (C13 * R11 + C23 * R12 + C33 * R13) * R33;
900 c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
901 R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
902 R23 * (C13 * R21 + C23 * R22 + C33 * R23);
903 c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
904 (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
905 (C13 * R21 + C23 * R22 + C33 * R23) * R33;
906 c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
907 R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
908 R33 * (C13 * R31 + C23 * R32 + C33 * R33);
920 float Nx = lm->normal.x;
921 float Ny = lm->normal.y;
922 float Nz = lm->normal.z;
924 newLandmark.
normal.
x = Nx * R11 + Ny * R12 + Nz * R13;
925 newLandmark.
normal.
y = Nx * R21 + Ny * R22 + Nz * R23;
926 newLandmark.
normal.
z = Nx * R31 + Ny * R32 + Nz * R33;
928 newLandmark.
ID = lm->ID;
930 newLandmark.
features = lm->features;
935 landmarks.push_back(newLandmark);
942 void CLandmarksMap::fuseWith(
CLandmarksMap& other,
bool justInsertAllOfThem)
948 std::vector<bool> otherCorrs(other.
size(),
false);
950 TMatchingPairList::iterator corrIt;
956 unsigned int nRemoved = 0;
958 if (!justInsertAllOfThem)
962 computeMatchingWith3DLandmarks(&other, corrs, corrsRatio, otherCorrs);
966 for (corrIt = corrs.begin(); corrIt != corrs.end(); corrIt++)
968 thisLM = landmarks.get(corrIt->this_idx);
979 landmarks.isToBeModified(corrIt->this_idx);
986 landmarks.hasBeenModified(corrIt->this_idx);
994 for (i = 0; i <
n; i++)
1007 if (!justInsertAllOfThem)
1012 n = landmarks.size();
1013 for (i =
n - 1; i >= 0; i--)
1015 if (landmarks.get(i)->getType() !=
1020 std::chrono::duration_cast<std::chrono::milliseconds>(
1021 lastestTime - landmarks.get(i)->timestampLastSeen)
1023 if (dt > fuseOptions.ellapsedTime &&
1024 landmarks.get(i)->seenTimesCount < fuseOptions.minTimesSeen)
1036 "[CLandmarksMap::fuseWith] %u fused/ %u new/ %u removed -> %u " 1038 static_cast<unsigned int>(corrs.size()),
1039 static_cast<unsigned int>(other.
size() - corrs.size()),
1040 static_cast<unsigned int>(nRemoved),
1041 static_cast<unsigned int>(landmarks.size()));
1044 f,
"%u\t%u\t%u\t%u\n", static_cast<unsigned int>(corrs.size()),
1045 static_cast<unsigned int>(other.
size() - corrs.size()),
1046 static_cast<unsigned int>(nRemoved),
1047 static_cast<unsigned int>(landmarks.size()));
1057 void CLandmarksMap::computeMatchingWith3DLandmarks(
1060 std::vector<bool>& otherCorrespondences)
const 1064 TSequenceLandmarks::const_iterator thisIt, otherIt;
1065 unsigned int nThis, nOther;
1068 unsigned int i,
n, j, k;
1070 double lik_dist, lik_desc, lik, maxLik;
1074 std::vector<bool> thisLandmarkAssigned;
1075 double K_desc = 0.0;
1076 double K_dist = 0.0;
1082 nThis = landmarks.size();
1086 thisLandmarkAssigned.resize(nThis,
false);
1089 correspondences.clear();
1090 otherCorrespondences.clear();
1091 otherCorrespondences.resize(nOther,
false);
1092 correspondencesRatio = 0;
1098 switch (insertionOptions.SIFTMatching3DMethod)
1106 -0.5 /
square(likelihoodOptions.SIFTs_sigma_descriptor_dist);
1107 K_dist = -0.5 /
square(likelihoodOptions.SIFTs_mahaDist_std);
1114 otherIt != anotherMap->
landmarks.
end(); otherIt++, k++)
1117 otherIt->getPose(pointPDF_k);
1119 if (otherIt->getType() ==
featSIFT)
1125 for (j = 0, thisIt = landmarks.begin();
1126 thisIt != landmarks.end(); thisIt++, j++)
1128 if (thisIt->getType() ==
featSIFT &&
1129 thisIt->features.size() ==
1130 otherIt->features.size() &&
1131 !thisIt->features.empty() &&
1132 thisIt->features[0].descriptors.SIFT->size() ==
1133 otherIt->features[0].descriptors.SIFT->size())
1139 thisIt->getPose(pointPDF_j);
1146 double distMahaFlik2;
1154 pointPDF_k.
mean.z() - pointPDF_j.
mean.z();
1164 lik_dist = exp(K_dist * distMahaFlik2);
1167 if (lik_dist > 1e-2)
1180 mPair(thisIt->ID, otherIt->ID);
1182 if (CLandmarksMap::_mEDD[mPair] == 0)
1184 n = otherIt->features[0]
1185 .descriptors.SIFT->size();
1187 for (i = 0; i <
n; i++)
1189 (*otherIt->features[0]
1190 .descriptors.SIFT)[i] -
1191 (*thisIt->features[0]
1192 .descriptors.SIFT)[i]);
1194 CLandmarksMap::_mEDD[mPair] = desc;
1198 desc = CLandmarksMap::_mEDD[mPair];
1201 lik_desc = exp(K_desc * desc);
1212 lik = lik_dist * lik_desc;
1235 if (maxLik > insertionOptions.SiftLikelihoodThreshold)
1239 if (!thisLandmarkAssigned[maxIdx])
1241 thisLandmarkAssigned[maxIdx] =
true;
1244 otherCorrespondences[k] =
true;
1247 match.
this_x = landmarks.get(maxIdx)->pose_mean.x;
1248 match.
this_y = landmarks.get(maxIdx)->pose_mean.y;
1249 match.
this_z = landmarks.get(maxIdx)->pose_mean.z;
1259 correspondences.push_back(match);
1268 correspondencesRatio =
1269 correspondences.size() /
static_cast<float>(nOther);
1286 ASSERT_(!landmarks.begin()->features.empty());
1289 .descriptors.SIFT->size();
1291 otherIt != anotherMap->
landmarks.
end(); otherIt++, k++)
1294 unsigned int mEDDidx = 0;
1295 for (j = 0, thisIt = landmarks.begin();
1296 thisIt != landmarks.end(); thisIt++, j++)
1300 for (i = 0; i < dLen; i++)
1302 (*otherIt->features[0].descriptors.SIFT)[i] -
1303 (*thisIt->features[0].descriptors.SIFT)[i]);
1314 if (mEDD > insertionOptions.SiftEDDThreshold)
1317 if (!thisLandmarkAssigned[mEDDidx])
1321 thisLandmarkAssigned[mEDDidx] =
true;
1324 otherCorrespondences[k] =
true;
1326 otherIt->getPose(pointPDF_k);
1327 thisIt->getPose(pointPDF_j);
1330 match.
this_x = landmarks.get(mEDDidx)->pose_mean.x;
1331 match.
this_y = landmarks.get(mEDDidx)->pose_mean.y;
1332 match.
this_z = landmarks.get(mEDDidx)->pose_mean.z;
1342 correspondences.push_back(match);
1350 correspondencesRatio =
1351 correspondences.size() /
static_cast<float>(nOther);
1367 FILE* f =
os::fopen(file.c_str(),
"wt");
1368 if (!f)
return false;
1377 for (
auto it = landmarks.begin(); it != landmarks.end(); ++it)
1380 f,
"%10f %10f %10f %4i %4u %10f", it->pose_mean.x, it->pose_mean.y,
1381 it->pose_mean.z, static_cast<int>(it->getType()),
1386 it->timestampLastSeen));
1390 ASSERT_(!it->features.empty());
1391 for (
unsigned char i : *it->features[0].descriptors.SIFT)
1409 bool CLandmarksMap::saveToMATLABScript3D(
1410 std::string file,
const char* style,
float confInterval)
const 1412 FILE* f =
os::fopen(file.c_str(),
"wt");
1413 if (!f)
return false;
1417 f,
"%%-------------------------------------------------------\n");
1418 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1419 os::fprintf(f,
"%% 'CLandmarksMap::saveToMATLABScript3D'\n");
1423 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1426 f,
"%%-------------------------------------------------------\n\n");
1431 for (
const auto& landmark : landmarks)
1434 f,
"m=[%.4f %.4f %.4f];", landmark.pose_mean.x,
1435 landmark.pose_mean.y, landmark.pose_mean.z);
1437 f,
"c=[%.8f %.8f %.8f;%.8f %.8f %.8f;%.8f %.8f %.8f]; ",
1438 landmark.pose_cov_11, landmark.pose_cov_12, landmark.pose_cov_13,
1439 landmark.pose_cov_12, landmark.pose_cov_22, landmark.pose_cov_23,
1440 landmark.pose_cov_13, landmark.pose_cov_23, landmark.pose_cov_33);
1444 "error_ellipse(c,m,'conf',%f,'style','%s','numPointsEllipse',10);" 1446 confInterval, style);
1449 os::fprintf(f,
"axis equal;grid on;xlabel('x'); ylabel('y'); zlabel('z');");
1459 bool CLandmarksMap::saveToMATLABScript2D(
1460 std::string file,
const char* style,
float stdCount)
1462 FILE* f =
os::fopen(file.c_str(),
"wt");
1463 if (!f)
return false;
1465 const int ELLIPSE_POINTS = 30;
1466 std::vector<float> X, Y, COS, SIN;
1467 std::vector<float>::iterator
x,
y, Cos, Sin;
1471 X.resize(ELLIPSE_POINTS);
1472 Y.resize(ELLIPSE_POINTS);
1473 COS.resize(ELLIPSE_POINTS);
1474 SIN.resize(ELLIPSE_POINTS);
1477 for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
1478 Cos++, Sin++, ang += (
M_2PI / (ELLIPSE_POINTS - 1)))
1486 f,
"%%-------------------------------------------------------\n");
1487 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1488 os::fprintf(f,
"%% 'CLandmarksMap::saveToMATLABScript2D'\n");
1492 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1495 f,
"%%-------------------------------------------------------\n\n");
1500 for (
auto& landmark : landmarks)
1503 cov(0, 0) = landmark.pose_cov_11;
1504 cov(1, 1) = landmark.pose_cov_22;
1505 cov(0, 1) =
cov(1, 0) = landmark.pose_cov_12;
1507 std::vector<double> eigvals;
1511 eigVal = eigVal.
array().sqrt().matrix();
1516 for (
x = X.begin(),
y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin();
1517 x != X.end();
x++,
y++, Cos++, Sin++)
1520 (landmark.pose_mean.x +
1521 stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
1523 (landmark.pose_mean.y +
1524 stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
1530 for (
x = X.begin();
x != X.end();
x++)
1536 for (
y = Y.begin();
y != Y.end();
y++)
1555 void CLandmarksMap::loadOccupancyFeaturesFrom2DRangeScan(
1557 unsigned int downSampleFactor)
1561 double J11, J12, J21, J22;
1572 sensorGlobalPose = *robotPose + obs.
sensorPose;
1587 double var_d =
square(0.005);
1588 double var_th =
square(dTh / 10.0);
1591 for (i = 0; i <
n; i += downSampleFactor, Th += downSampleFactor * dTh)
1610 newLandmark.
features[0].orientation = Th;
1611 newLandmark.
features[0].keypoint.octave = d;
1629 newLandmark.
pose_cov_11 = (J11 * J11 * var_th + J12 * J12 * var_d);
1630 newLandmark.
pose_cov_12 = (J11 * J21 * var_th + J12 * J22 * var_d);
1631 newLandmark.
pose_cov_22 = (J21 * J21 * var_th + J22 * J22 * var_d);
1637 landmarks.push_back(newLandmark);
1645 changeCoordinatesReference(sensorGlobalPose);
1655 double CLandmarksMap::computeLikelihood_RSLC_2007(
1662 TSequenceLandmarks::const_iterator itOther;
1669 double nFoundCorrs = 0;
1670 std::vector<int32_t>* corrs;
1671 unsigned int cx, cy, cx_1, cx_2, cy_1, cy_2;
1680 for (itOther =
s->landmarks.begin(); itOther !=
s->landmarks.end();
1683 itOther->getPose(poseOther);
1685 cx = cy = grid->
y2idx(itOther->pose_mean.y);
1687 cx_1 = max(0, grid->
x2idx(itOther->pose_mean.x - 0.10f));
1690 grid->
x2idx(itOther->pose_mean.x + 0.10f));
1691 cy_1 = max(0, grid->
y2idx(itOther->pose_mean.y - 0.10f));
1694 grid->
y2idx(itOther->pose_mean.y + 0.10f));
1705 for (cx = cx_1; cx <= cx_2; cx++)
1706 for (cy = cy_1; cy <= cy_2; cy++)
1710 if (!corrs->empty())
1711 for (
int& it : *corrs)
1713 lm = landmarks.get(it);
1719 if (fabs(lm->
pose_mean.
x - itOther->pose_mean.x) >
1721 fabs(lm->
pose_mean.
y - itOther->pose_mean.y) >
1738 PrNoCorr *= 1 - corr;
1745 nFoundCorrs += 1 - PrNoCorr;
1776 lik *= 1 - PrNoCorr;
1780 lik = nFoundCorrs /
static_cast<double>(
s->size());
1795 CLandmarksMap::TCustomSequenceLandmarks::TCustomSequenceLandmarks()
1796 : m_landmarks(), m_grid(-10.0f, 10.0f, -10.0f, 10.f, 0.20f)
1803 m_landmarks.clear();
1808 m_largestDistanceFromOriginIsUpdated =
false;
1814 std::vector<int32_t> dummyEmpty;
1820 max(m_grid.getYMax(), l.
pose_mean.
y + 0.1), dummyEmpty);
1822 m_landmarks.push_back(l);
1827 cell->push_back(m_landmarks.size() - 1);
1829 m_largestDistanceFromOriginIsUpdated =
false;
1834 return &m_landmarks[indx];
1838 unsigned int indx)
const 1840 return &m_landmarks[indx];
1845 std::vector<int32_t>* cell = m_grid.cellByPos(
1846 m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1848 std::vector<int32_t>::iterator it;
1849 for (it = cell->begin(); it != cell->end(); it++)
1851 if (*it == static_cast<int>(indx))
1858 m_largestDistanceFromOriginIsUpdated =
false;
1863 m_landmarks.erase(m_landmarks.begin() + indx);
1864 m_largestDistanceFromOriginIsUpdated =
false;
1869 std::vector<int32_t> dummyEmpty;
1873 min(m_grid.getXMin(), m_landmarks[indx].pose_mean.x),
1874 max(m_grid.getXMax(), m_landmarks[indx].pose_mean.x),
1875 min(m_grid.getYMin(), m_landmarks[indx].pose_mean.y),
1876 max(m_grid.getYMax(), m_landmarks[indx].pose_mean.y), dummyEmpty);
1879 std::vector<int32_t>* cell = m_grid.cellByPos(
1880 m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1881 cell->push_back(indx);
1882 m_largestDistanceFromOriginIsUpdated =
false;
1889 TSequenceLandmarks::iterator it;
1891 double min_x = -10.0, max_x = 10.0;
1892 double min_y = -10.0, max_y = 10.0;
1893 std::vector<int32_t> dummyEmpty;
1899 for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1902 min_x =
min(min_x, it->pose_mean.x);
1903 max_x = max(max_x, it->pose_mean.x);
1904 min_y =
min(min_y, it->pose_mean.y);
1905 max_y = max(max_y, it->pose_mean.y);
1907 m_grid.resize(min_x, max_x, min_y, max_y, dummyEmpty);
1910 for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1913 std::vector<int32_t>* cell =
1914 m_grid.cellByPos(it->pose_mean.x, it->pose_mean.y);
1915 cell->push_back(idx);
1918 m_largestDistanceFromOriginIsUpdated =
false;
1929 if (!m_largestDistanceFromOriginIsUpdated)
1932 float maxDistSq = 0, d;
1933 for (
const auto& it : *
this)
1937 maxDistSq = max(d, maxDistSq);
1940 m_largestDistanceFromOrigin = sqrt(maxDistSq);
1941 m_largestDistanceFromOriginIsUpdated =
true;
1943 return m_largestDistanceFromOrigin;
1951 std::vector<bool>* otherCorrespondences)
1955 unsigned long distDesc;
1956 double likByDist, likByDesc;
1958 std::vector<unsigned char>::iterator it1, it2;
1963 unsigned int idx1, idx2;
1965 CMatrixD dij(1, 3), Cij(3, 3), Cij_1;
1966 double distMahaFlik2;
1977 TSequenceLandmarks::iterator lm1, lm2;
1980 lm1 += decimation, idx1 += decimation)
1985 lm1->getPose(lm1_pose);
1996 lm2->getPose(lm2_pose);
2001 dij(0, 0) = lm1->pose_mean.x - lm2->pose_mean.x;
2002 dij(0, 1) = lm1->pose_mean.y - lm2->pose_mean.y;
2003 dij(0, 2) = lm1->pose_mean.z - lm2->pose_mean.z;
2015 likByDist = exp(K_dist * distMahaFlik2);
2017 if (likByDist > 1e-2)
2029 mPair(lm2->ID, lm1->ID);
2037 !lm1->features.empty() &&
2038 !lm2->features.empty());
2041 .descriptors.SIFT->size() ==
2043 .descriptors.SIFT->size());
2045 for (it1 = lm1->features[0]
2046 .descriptors.SIFT->begin(),
2047 it2 = lm2->features[0]
2048 .descriptors.SIFT->begin();
2049 it1 != lm1->features[0]
2050 .descriptors.SIFT->end();
2052 distDesc +=
square(*it1 - *it2);
2060 distDesc = (
unsigned long)
2063 likByDesc = exp(K_desc * distDesc);
2064 lik_i += likByDist *
2076 lik *= (0.1 + 0.9 * lik_i);
2085 TMatchingPairList::iterator itCorr;
2090 if (correspondences ==
nullptr)
2092 "When using this method with SIFTLikelihoodMethod = 1, " 2093 "TMatchingPairList *correspondence can not be NULL");
2095 if (otherCorrespondences ==
nullptr)
2097 "When using this method with SIFTLikelihoodMethod = 1, " 2098 "std::vector<bool> *otherCorrespondences can not be NULL");
2100 for (itCorr = correspondences->begin();
2101 itCorr != correspondences->end(); itCorr++)
2120 lik *= exp(-0.5 * dist);
2125 for (k = 1; k <= (theMap->
size() - correspondences->size()); k++)
2143 SIFT_feat_options(vision::
featSIFT)
2153 "\n----------- [CLandmarksMap::TInsertionOptions] ------------ \n\n");
2156 "insert_SIFTs_from_monocular_images = %c\n",
2157 insert_SIFTs_from_monocular_images ?
'Y' :
'N');
2159 "insert_SIFTs_from_stereo_images = %c\n",
2160 insert_SIFTs_from_stereo_images ?
'Y' :
'N');
2162 "insert_Landmarks_from_range_scans = %c\n",
2163 insert_Landmarks_from_range_scans ?
'Y' :
'N');
2166 "SiftCorrRatioThreshold = %f\n",
2167 SiftCorrRatioThreshold);
2169 "SiftLikelihoodThreshold = %f\n",
2170 SiftLikelihoodThreshold);
2172 "SiftEDDThreshold = %f\n", SiftEDDThreshold);
2174 "SIFTMatching3DMethod = %d\n", SIFTMatching3DMethod);
2176 "SIFTLikelihoodMethod = %d\n", SIFTLikelihoodMethod);
2179 "SIFTsLoadDistanceOfTheMean = %f\n",
2180 SIFTsLoadDistanceOfTheMean);
2182 "SIFTsLoadEllipsoidWidth = %f\n",
2183 SIFTsLoadEllipsoidWidth);
2186 "SIFTs_stdXY = %f\n", SIFTs_stdXY);
2188 "SIFTs_stdDisparity = %f\n", SIFTs_stdDisparity);
2191 "SIFTs_numberOfKLTKeypoints = %i\n",
2192 SIFTs_numberOfKLTKeypoints);
2194 "SIFTs_stereo_maxDepth = %f\n",
2195 SIFTs_stereo_maxDepth);
2197 "SIFTs_epipolar_TH = %f\n", SIFTs_epipolar_TH);
2199 "PLOT_IMAGES = %c\n",
2200 PLOT_IMAGES ?
'Y' :
'N');
2202 SIFT_feat_options.dumpToTextStream(out);
2213 insert_SIFTs_from_monocular_images =
iniFile.read_bool(
2214 section.c_str(),
"insert_SIFTs_from_monocular_images",
2215 insert_SIFTs_from_monocular_images);
2216 insert_SIFTs_from_stereo_images =
iniFile.read_bool(
2217 section.c_str(),
"insert_SIFTs_from_stereo_images",
2218 insert_SIFTs_from_stereo_images);
2219 insert_Landmarks_from_range_scans =
iniFile.read_bool(
2220 section.c_str(),
"insert_Landmarks_from_range_scans",
2221 insert_Landmarks_from_range_scans);
2222 SiftCorrRatioThreshold =
iniFile.read_float(
2223 section.c_str(),
"SiftCorrRatioThreshold", SiftCorrRatioThreshold);
2224 SiftLikelihoodThreshold =
iniFile.read_float(
2225 section.c_str(),
"SiftLikelihoodThreshold", SiftLikelihoodThreshold);
2226 SiftEDDThreshold =
iniFile.read_float(
2227 section.c_str(),
"SiftEDDThreshold", SiftEDDThreshold);
2228 SIFTMatching3DMethod =
iniFile.read_int(
2229 section.c_str(),
"SIFTMatching3DMethod", SIFTMatching3DMethod);
2230 SIFTLikelihoodMethod =
iniFile.read_int(
2231 section.c_str(),
"SIFTLikelihoodMethod", SIFTLikelihoodMethod);
2232 SIFTsLoadDistanceOfTheMean =
iniFile.read_float(
2233 section.c_str(),
"SIFTsLoadDistanceOfTheMean",
2234 SIFTsLoadDistanceOfTheMean);
2235 SIFTsLoadEllipsoidWidth =
iniFile.read_float(
2236 section.c_str(),
"SIFTsLoadEllipsoidWidth", SIFTsLoadEllipsoidWidth);
2238 iniFile.read_float(section.c_str(),
"SIFTs_stdXY", SIFTs_stdXY);
2239 SIFTs_stdDisparity =
iniFile.read_float(
2240 section.c_str(),
"SIFTs_stdDisparity", SIFTs_stdDisparity);
2241 SIFTs_numberOfKLTKeypoints =
iniFile.read_int(
2242 section.c_str(),
"SIFTs_numberOfKLTKeypoints",
2243 SIFTs_numberOfKLTKeypoints);
2244 SIFTs_stereo_maxDepth =
iniFile.read_float(
2245 section.c_str(),
"SIFTs_stereo_maxDepth", SIFTs_stereo_maxDepth);
2246 SIFTs_epipolar_TH =
iniFile.read_float(
2247 section.c_str(),
"SIFTs_epipolar_TH", SIFTs_epipolar_TH);
2249 iniFile.read_bool(section.c_str(),
"PLOT_IMAGES", PLOT_IMAGES);
2251 SIFT_feat_options.loadFromConfigFile(
iniFile, section);
2258 : SIFT_feat_options(vision::
featSIFT),
2273 std::ostream& out)
const 2276 "\n----------- [CLandmarksMap::TLikelihoodOptions] ------------ \n\n");
2279 "rangeScan2D_decimation = %i\n",
2280 rangeScan2D_decimation);
2282 "SIFTs_sigma_euclidean_dist = %f\n",
2283 SIFTs_sigma_euclidean_dist);
2285 "SIFTs_sigma_descriptor_dist = %f\n",
2286 SIFTs_sigma_descriptor_dist);
2288 "SIFTs_mahaDist_std = %f\n", SIFTs_mahaDist_std);
2290 "SIFTs_decimation = %i\n", SIFTs_decimation);
2292 "SIFTnullCorrespondenceDistance = %f\n",
2293 SIFTnullCorrespondenceDistance);
2295 "beaconRangesStd = %f\n", beaconRangesStd);
2297 "beaconRangesUseObservationStd = %c\n",
2298 beaconRangesUseObservationStd ?
'Y' :
'N');
2300 "extRobotPoseStd = %f\n", extRobotPoseStd);
2303 "GPSOrigin:LATITUDE = %f\n", GPSOrigin.latitude);
2305 "GPSOrigin:LONGITUDE = %f\n", GPSOrigin.longitude);
2307 "GPSOrigin:ALTITUDE = %f\n", GPSOrigin.altitude);
2309 "GPSOrigin:Rotation_Angle = %f\n", GPSOrigin.ang);
2311 "GPSOrigin:x_shift = %f\n", GPSOrigin.x_shift);
2313 "GPSOrigin:y_shift = %f\n", GPSOrigin.y_shift);
2315 "GPSOrigin:min_sat = %i\n", GPSOrigin.min_sat);
2318 "GPS_sigma = %f (m)\n", GPS_sigma);
2320 SIFT_feat_options.dumpToTextStream(out);
2331 rangeScan2D_decimation =
iniFile.read_int(
2332 section.c_str(),
"rangeScan2D_decimation", rangeScan2D_decimation);
2333 SIFTs_sigma_euclidean_dist =
iniFile.read_double(
2334 section.c_str(),
"SIFTs_sigma_euclidean_dist",
2335 SIFTs_sigma_euclidean_dist);
2336 SIFTs_sigma_descriptor_dist =
iniFile.read_double(
2337 section.c_str(),
"SIFTs_sigma_descriptor_dist",
2338 SIFTs_sigma_descriptor_dist);
2339 SIFTs_mahaDist_std =
iniFile.read_float(
2340 section.c_str(),
"SIFTs_mahaDist_std", SIFTs_mahaDist_std);
2342 iniFile.read_int(section.c_str(),
"SIFTs_decimation", SIFTs_decimation);
2343 SIFTnullCorrespondenceDistance =
iniFile.read_float(
2344 section.c_str(),
"SIFTnullCorrespondenceDistance",
2345 SIFTnullCorrespondenceDistance);
2347 GPSOrigin.latitude =
iniFile.read_double(
2348 section.c_str(),
"GPSOriginLatitude", GPSOrigin.latitude);
2349 GPSOrigin.longitude =
iniFile.read_double(
2350 section.c_str(),
"GPSOriginLongitude", GPSOrigin.longitude);
2351 GPSOrigin.altitude =
iniFile.read_double(
2352 section.c_str(),
"GPSOriginAltitude", GPSOrigin.altitude);
2354 iniFile.read_double(section.c_str(),
"GPSOriginAngle", GPSOrigin.ang) *
2356 GPSOrigin.x_shift =
iniFile.read_double(
2357 section.c_str(),
"GPSOriginXshift", GPSOrigin.x_shift);
2358 GPSOrigin.y_shift =
iniFile.read_double(
2359 section.c_str(),
"GPSOriginYshift", GPSOrigin.y_shift);
2361 iniFile.read_int(section.c_str(),
"GPSOriginMinSat", GPSOrigin.min_sat);
2363 GPS_sigma =
iniFile.read_float(section.c_str(),
"GPSSigma", GPS_sigma);
2366 iniFile.read_float(section.c_str(),
"beaconRangesStd", beaconRangesStd);
2367 beaconRangesUseObservationStd =
iniFile.read_bool(
2368 section.c_str(),
"beaconRangesUseObservationStd",
2369 beaconRangesUseObservationStd);
2372 iniFile.read_float(section.c_str(),
"extRobotPoseStd", extRobotPoseStd);
2374 SIFT_feat_options.loadFromConfigFile(
iniFile, section);
2392 const CPose3D& in_robotPose,
const CPoint3D& in_sensorLocationOnRobot,
2395 TSequenceLandmarks::const_iterator it;
2401 point3D = in_robotPose + in_sensorLocationOnRobot;
2413 it->getPose(beaconPDF);
2414 beacon3D = beaconPDF.
mean;
2432 out_Observations.
sensedData.push_back(newMeas);
2458 std::make_shared<opengl::CGridPlaneXY>(-100, 100, -100, 100, 0, 1);
2485 landmark.getPose(pointGauss);
2487 ellip->setPose(pointGauss.
mean);
2488 ellip->setCovMatrix(pointGauss.
cov);
2489 ellip->enableDrawSolid3D(
false);
2490 ellip->setQuantiles(3.0);
2491 ellip->set3DsegmentsCount(10);
2492 ellip->setColor(0, 0, 1);
2494 mrpt::format(
"LM.ID=%u", static_cast<unsigned int>(landmark.ID)));
2495 ellip->enableShowName(
true);
2497 outObj->insert(ellip);
2510 for (
const auto& m_landmark : m_landmarks)
2512 if (m_landmark.ID == ID)
return &m_landmark;
2529 unsigned int ID)
const 2531 for (
const auto& m_landmark : m_landmarks)
2533 if (m_landmark.ID == ID)
return &m_landmark;
2566 otherMap = static_cast<const CLandmarksMap*>(otherMap2);
2568 if (!otherMap)
return 0;
2571 std::deque<CPointPDFGaussian::Ptr> poses3DThis, poses3DOther;
2572 std::deque<CPointPDFGaussian::Ptr>::iterator itPoseThis, itPoseOther;
2575 size_t nOther = otherMap->landmarks.size();
2576 size_t otherLandmarkWithCorrespondence = 0;
2579 if (!nThis)
return 0;
2580 if (!nOther)
return 0;
2585 float Tx = pose3DMatrix(0, 3);
2586 float Ty = pose3DMatrix(1, 3);
2587 float Tz = pose3DMatrix(2, 3);
2597 otherMap->landmarks.getLargestDistanceFromOrigin() + 1.0f))
2601 poses3DOther.resize(nOther);
2602 for (
size_t i = 0; i < nOther; i++)
2603 poses3DOther[i] = std::make_shared<CPointPDFGaussian>();
2605 poses3DThis.resize(nThis);
2606 for (
size_t i = 0; i < nThis; i++)
2607 poses3DThis[i] = std::make_shared<CPointPDFGaussian>();
2610 for (itOther = otherMap->landmarks.begin(),
2611 itPoseOther = poses3DOther.begin();
2612 itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2614 itOther->getPose(**itPoseOther);
2615 (*itPoseOther)->changeCoordinatesReference(otherMapPose);
2619 for (itThis =
landmarks.
begin(), itPoseThis = poses3DThis.begin();
2622 itThis->getPose(**itPoseThis);
2626 for (itOther = otherMap->landmarks.begin(),
2627 itPoseOther = poses3DOther.begin();
2628 itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2630 for (itThis =
landmarks.
begin(), itPoseThis = poses3DThis.begin();
2636 (*itPoseThis)->mean.x() - (*itPoseOther)->mean.x(),
2637 (*itPoseThis)->mean.y() - (*itPoseOther)->mean.y(),
2638 (*itPoseThis)->mean.z() - (*itPoseOther)->mean.z());
2647 if (distMaha <
params.maxMahaDistForCorr)
2650 if (!itThis->features.empty() && !itOther->features.empty() &&
2651 itThis->features[0].descriptors.SIFT->size() ==
2652 itOther->features[0].descriptors.SIFT->size())
2654 unsigned long descrDist = 0;
2655 std::vector<unsigned char>::const_iterator it1, it2;
2656 for (it1 = itThis->features[0].descriptors.SIFT->begin(),
2657 it2 = itOther->features[0].descriptors.SIFT->begin();
2658 it1 != itThis->features[0].descriptors.SIFT->end();
2660 descrDist +=
square(*it1 - *it2);
2663 sqrt(static_cast<float>(descrDist)) /
2664 itThis->features[0].descriptors.SIFT->size();
2666 if (descrDist_f < 1.5f)
2668 otherLandmarkWithCorrespondence++;
2677 return static_cast<float>(otherLandmarkWithCorrespondence) / nOther;
2695 const CPose3D& in_robotPose,
const CPose3D& in_sensorLocationOnRobot,
2697 const float in_stdRange,
const float in_stdYaw,
const float in_stdPitch,
2698 std::vector<size_t>* out_real_associations,
2699 const double spurious_count_mean,
const double spurious_count_std)
const 2701 TSequenceLandmarks::const_iterator it;
2707 if (out_real_associations) out_real_associations->clear();
2710 const CPose3D point3D = in_robotPose +
CPose3D(in_sensorLocationOnRobot);
2727 it->getPose(beaconPDF);
2748 if (sensorDetectsIDs)
2757 out_Observations.
sensedData.push_back(newMeas);
2759 if (out_real_associations)
2760 out_real_associations->push_back(idx);
2765 spurious_count_mean, spurious_count_std);
2766 size_t nSpurious = 0;
2767 if (spurious_count_std != 0 || spurious_count_mean != 0)
2773 for (
size_t i = 0; i < nSpurious; i++)
2786 const double pitch =
2801 out_Observations.
sensedData.push_back(newMeas);
2803 if (out_real_associations)
2804 out_real_associations->push_back(
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
A namespace of pseudo-random numbers generators of diferent distributions.
float fieldOfView_yaw
Information about the.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
internal::TSequenceLandmarks::iterator iterator
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
TFuseOptions()
Initialization.
Parameters for CMetricMap::compute3DMatchingRatio()
GLuint GLuint GLsizei count
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
size_t size() const
Returns the stored landmarks count.
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts
int y2idx(double y) const
A 2D grid of dynamic size which stores any kind of data at each cell.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
void createOneFeature()
Creates one feature in the vector "features", calling the appropriate constructor of the smart pointe...
double x
X,Y,Z coordinates.
float range
The sensed landmark distance, in meters.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
TInternalFeatList::iterator iterator
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
#define THROW_EXCEPTION(msg)
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
static bool _maxIDUpdated
Non-defined feature (also used for Occupancy features)
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
const CLandmark * getByID(CLandmark::TLandmarkID ID) const
Returns the landmark with a given landmrk ID, or nullptr if not found.
int void fclose(FILE *f)
An OS-independent version of fclose.
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
CPoint3D mean
The mean value.
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
long round_long(const T value)
Returns the closer integer (long) to x.
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
uint32_t satellitesUsed
The number of satelites used to compute this estimation.
double norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
double extractDayTimeFromTimestamp(const mrpt::system::TTimeStamp t)
Returns the number of seconds ellapsed from midnight in the given timestamp.
An observation providing an alternative robot pose from an external source.
double pitch() const
Get the PITCH angle (in radians)
double yaw() const
Get the YAW angle (in radians)
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
Scale Invariant Feature Transform [LOWE'04].
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H^t * C * H (with a row vector H and a symmetric matrix C)
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.
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
static Ptr Create(Args &&... args)
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.
TMapGenericParams genericMapParams
Common params to all maps.
float epipolar_TH
Epipolar constraint (rows of pixels)
A especial case: this is not an image feature, but a 2D/3D beacon (used for range-only SLAM from mrpt...
float minZ
Maximum allowed distance.
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.
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
GLsizei GLsizei GLuint * obj
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a noisy reading toward each of the beacons in the landmarks map, if any.
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or nullptr if not found.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
std::vector< CLandmark > TSequenceLandmarks
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
float stdPixel
Standard deviation of the error in feature detection.
int64_t TLandmarkID
The type for the IDs of landmarks.
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
T square(const T x)
Inline function for the square of a number.
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
#define ASSERT_(f)
Defines an assertion mechanism.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
mrpt::maps::CLandmark::TLandmarkID getMapMaxID()
double altitude_meters
The measured altitude, in meters (A).
Matching by Euclidean distance between SIFT descriptors.
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 setPose(const mrpt::poses::CPointPDFGaussian &p)
Sets the pose from an object:
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
A class for storing a map of 3D probabilistic landmarks.
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
void erase(unsigned int indx)
mrpt::img::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
Parameters associated to a stereo system.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
std::deque< TMeasurement > sensedData
The list of observed ranges.
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch )...
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
double computeLikelihood_SIFT_LandmarkMap(CLandmarksMap *map, mrpt::tfest::TMatchingPairList *correspondences=nullptr, std::vector< bool > *otherCorrespondences=nullptr)
Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
Classes for computer vision, detectors, features, etc.
void getPose(mrpt::poses::CPointPDFGaussian &p) const
Returns the pose as an object:
mrpt::img::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
double x() const
Common members of all points & poses classes.
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
float SIFTnullCorrespondenceDistance
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
double norm() const
Returns the euclidean norm of vector: .
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.
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
mrpt::system::TTimeStamp timestampLastSeen
The last time that this landmark was observed.
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
mrpt::poses::CPose3D sensorLocationOnRobot
The position of the sensor on the robot.
Each one of the measurements:
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
static std::map< std::pair< mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID >, double > _mEDD
Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks.
float minSensorDistance
Info about sensor.
mrpt::math::TPoint3D asTPoint() const
void hasBeenModified(unsigned int indx)
float sensedDistance
The sensed range itself (in meters).
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts
void hasBeenModifiedAll()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
Virtual base class for "archives": classes abstracting I/O streams.
Declares a virtual base class for all metric maps storage classes.
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).
Declares a class that represents any robot's observation.
mrpt::math::TPoint3D normal
The "normal" to the landmark, i.e.
std::deque< TPairIdBeacon > initialBeacons
Initial contents of the map, especified by a set of 3D Beacons with associated IDs.
void loadSiftFeaturesFromStereoImageObservation(const mrpt::obs::CObservationStereoImages &obs, mrpt::maps::CLandmark::TLandmarkID fID, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
float maxZ
Maximum allowed distance.
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
size_t matchFeatures(const CFeatureList &list1, const CFeatureList &list2, CMatchedFeatureList &matches, const TMatchingOptions &options=TMatchingOptions(), const TStereoSystemParams ¶ms=TStereoSystemParams())
Find the matches between two lists of features which must be of the same type.
CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
A structure containing options for the matching.
TInsertionOptions()
Initilization of default parameters.
std::vector< mrpt::vision::CFeature > features
The set of features from which the landmark comes.
void loadSiftFeaturesFromImageObservation(const mrpt::obs::CObservationImage &obs, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an image observation (Previous content...
content_t fields
Message content, accesible by individual fields.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
GLsizei GLsizei GLchar * source
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
int x2idx(double x) const
Transform a coordinate values into cell indexes.
#define INVALID_LANDMARK_ID
Used for CObservationBearingRange::TMeasurement::beaconID and others.
void auxParticleFilterCleanUp() override
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
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.
Each one of the measurements.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
CLandmark * get(unsigned int indx)
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
internal::TSequenceLandmarks::const_iterator const_iterator
void simulateRangeBearingReadings(const mrpt::poses::CPose3D &robotPose, const mrpt::poses::CPose3D &sensorLocationOnRobot, mrpt::obs::CObservationBearingRange &observations, bool sensorDetectsIDs=true, const float stdRange=0.01f, const float stdYaw=mrpt::DEG2RAD(0.1f), const float stdPitch=mrpt::DEG2RAD(0.1f), std::vector< size_t > *real_associations=nullptr, const double spurious_count_mean=0, const double spurious_count_std=0) const
Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the ...
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
bool saveToMATLABScript3D(std::string file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::img::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
TMatchingMethod matching_method
Matching method.
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
unsigned __int32 uint32_t
float sensor_std_range
Taken into account only if validCovariances=false: the standard deviation of the sensor noise model f...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
void clear()
Clear the contents of this container.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
void isToBeModified(unsigned int indx)
uint32_t seenTimesCount
The number of times that this landmark has been seen.
mrpt::math::TPoint3D pixelTo3D(const mrpt::img::TPixelCoordf &xy, const mrpt::math::CMatrixDouble33 &A)
Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates...
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
void loadOccupancyFeaturesFrom2DRangeScan(const mrpt::obs::CObservation2DRangeScan &obs, const mrpt::poses::CPose3D *robotPose=nullptr, unsigned int downSampleFactor=1)
Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous cont...
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
void setDiagonal(const std::size_t N, const Scalar value)
Resize to NxN, set all entries to zero, except the main diagonal which is set to value ...
A gaussian distribution for 3D points.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
double SIFTs_sigma_descriptor_dist
A class for storing images as grayscale or RGB bitmaps.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
float stdDisp
Standard deviation of the error in disparity computation.
TLandmarkID ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
TMeasurementList sensedData
The list of observed ranges:
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOptions