30 #include <Eigen/Dense>
50 CLandmarksMap::TMapDefinition::TMapDefinition() =
default;
51 void CLandmarksMap::TMapDefinition::loadFromConfigFile_map_specific(
53 const std::string& sectionNamePrefix)
56 const std::string sSectCreation =
57 sectionNamePrefix + string(
"_creationOpts");
58 this->initialBeacons.clear();
59 const unsigned int nBeacons = source.
read_int(sSectCreation,
"nBeacons", 0);
60 for (
unsigned int q = 1; q <= nBeacons; q++)
73 this->initialBeacons.push_back(newPair);
76 insertionOpts.loadFromConfigFile(
77 source, sectionNamePrefix +
string(
"_insertOpts"));
78 likelihoodOpts.loadFromConfigFile(
79 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
82 void CLandmarksMap::TMapDefinition::dumpToTextStream_map_specific(
83 std::ostream&
out)
const
86 "number of initial beacons = %u\n",
87 (
int)initialBeacons.size());
89 out <<
" ID (X,Y,Z)\n";
90 out <<
"--------------------------------------------------------\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; }
152 uint32_t n = landmarks.size();
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 const double sensorStd = likelihoodOptions.beaconRangesUseObservationStd
258 : likelihoodOptions.beaconRangesStd;
260 const auto unif_val =
261 std::log(1.0 / (o.maxSensorDistance - o.minSensorDistance));
266 for (
const auto& meas : o.sensedData)
269 unsigned int sensedID = meas.beaconID;
272 if (std::isnan(meas.sensedDistance))
continue;
275 const auto point3D = robotPose3D + meas.sensorLocationOnRobot;
277 for (
const auto& lm : landmarks)
279 if ((lm.getType() !=
featBeacon) || (lm.ID != sensedID))
282 lm.getPose(beaconPDF);
283 const auto& beacon3D = beaconPDF.
mean;
285 const double expectedRange = point3D.
distanceTo(beacon3D);
286 const double sensedDist =
287 std::max<double>(.0, meas.sensedDistance);
292 mrpt::square((expectedRange - sensedDist) / sensorStd));
298 if (!found && 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 [[maybe_unused]]
const CPose2D& otherMapPose,
564 [[maybe_unused]]
float maxDistForCorrespondence,
565 [[maybe_unused]]
float maxAngularDistForCorrespondence,
566 [[maybe_unused]]
const CPose2D& angularDistPivotPoint,
568 [[maybe_unused]]
float& correspondencesRatio,
569 [[maybe_unused]]
float* sumSqrDist,
570 [[maybe_unused]]
bool onlyKeepTheClosest,
571 [[maybe_unused]]
bool onlyUniqueRobust)
const
576 CPose3D otherMapPose3D(otherMapPose);
578 correspondencesRatio = 0;
582 const auto* otherMap2 =
dynamic_cast<const CLandmarksMap*
>(otherMap);
583 std::vector<bool> otherCorrespondences;
589 computeMatchingWith3DLandmarks(
590 otherMap2, correspondences, correspondencesRatio, otherCorrespondences);
598 void CLandmarksMap::loadSiftFeaturesFromImageObservation(
604 float d = insertionOptions.SIFTsLoadDistanceOfTheMean;
605 float width = insertionOptions.SIFTsLoadEllipsoidWidth;
630 for (sift = siftList.
begin(); sift != siftList.
end(); sift++)
641 landmark3DPositionPDF.
mean *= d;
647 D(0, 0) =
square(0.5 * d);
660 Normal *= -1 / Normal.
norm();
673 landmarks.push_back(lm);
681 void CLandmarksMap::loadSiftFeaturesFromStereoImageObservation(
696 fExt.options = feat_options;
703 insertionOptions.SIFTs_numberOfKLTKeypoints);
706 insertionOptions.SIFTs_numberOfKLTKeypoints);
710 matchingOptions.
epipolar_TH = insertionOptions.SIFTs_epipolar_TH;
711 matchingOptions.
EDD_RATIO = insertionOptions.SiftCorrRatioThreshold;
712 matchingOptions.
maxEDD_TH = insertionOptions.SiftEDDThreshold;
714 leftSiftList, rightSiftList, matchesList, matchingOptions);
716 if (insertionOptions.PLOT_IMAGES)
718 std::cerr <<
"Warning: insertionOptions.PLOT_IMAGES has no effect "
719 "since MRPT 0.9.1\n";
734 stereoParams.
stdPixel = insertionOptions.SIFTs_stdXY;
735 stereoParams.
stdDisp = insertionOptions.SIFTs_stdDisparity;
737 stereoParams.
minZ = 0.0f;
738 stereoParams.
maxZ = insertionOptions.SIFTs_stereo_maxDepth;
740 size_t numM = matchesList.size();
745 for (ii = landmarks.begin(); ii != landmarks.end(); ii++)
748 (*ii).seenTimesCount = 1;
752 "%u (out of %u) corrs!\n",
static_cast<unsigned>(landmarks.size()),
753 static_cast<unsigned>(numM));
766 void CLandmarksMap::changeCoordinatesReference(
const CPose3D& newOrg)
768 TSequenceLandmarks::iterator lm;
774 double R11 = HM(0, 0);
775 double R12 = HM(0, 1);
776 double R13 = HM(0, 2);
777 double R21 = HM(1, 0);
778 double R22 = HM(1, 1);
779 double R23 = HM(1, 2);
780 double R31 = HM(2, 0);
781 double R32 = HM(2, 1);
782 double R33 = HM(2, 2);
784 double c11, c22, c33, c12, c13, c23;
788 for (lm = landmarks.begin(); lm != landmarks.end(); lm++)
794 float C11 = lm->pose_cov_11;
795 float C22 = lm->pose_cov_22;
796 float C33 = lm->pose_cov_33;
797 float C12 = lm->pose_cov_12;
798 float C13 = lm->pose_cov_13;
799 float C23 = lm->pose_cov_23;
802 c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
803 R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
804 R13 * (C13 * R11 + C23 * R12 + C33 * R13);
805 c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
806 (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
807 (C13 * R11 + C23 * R12 + C33 * R13) * R23;
808 c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
809 (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
810 (C13 * R11 + C23 * R12 + C33 * R13) * R33;
811 c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
812 R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
813 R23 * (C13 * R21 + C23 * R22 + C33 * R23);
814 c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
815 (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
816 (C13 * R21 + C23 * R22 + C33 * R23) * R33;
817 c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
818 R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
819 R33 * (C13 * R31 + C23 * R32 + C33 * R33);
822 lm->pose_cov_11 = c11;
823 lm->pose_cov_22 = c22;
824 lm->pose_cov_33 = c33;
825 lm->pose_cov_12 = c12;
826 lm->pose_cov_13 = c13;
827 lm->pose_cov_23 = c23;
831 float Nx = lm->normal.x;
832 float Ny = lm->normal.y;
833 float Nz = lm->normal.z;
835 lm->normal.x = Nx * R11 + Ny * R12 + Nz * R13;
836 lm->normal.y = Nx * R21 + Ny * R22 + Nz * R23;
837 lm->normal.z = Nx * R31 + Ny * R32 + Nz * R33;
841 landmarks.hasBeenModifiedAll();
847 void CLandmarksMap::changeCoordinatesReference(
850 TSequenceLandmarks::const_iterator lm;
857 double R11 = HM(0, 0);
858 double R12 = HM(0, 1);
859 double R13 = HM(0, 2);
860 double R21 = HM(1, 0);
861 double R22 = HM(1, 1);
862 double R23 = HM(1, 2);
863 double R31 = HM(2, 0);
864 double R32 = HM(2, 1);
865 double R33 = HM(2, 2);
867 double c11, c22, c33, c12, c13, c23;
881 float C11 = lm->pose_cov_11;
882 float C22 = lm->pose_cov_22;
883 float C33 = lm->pose_cov_33;
884 float C12 = lm->pose_cov_12;
885 float C13 = lm->pose_cov_13;
886 float C23 = lm->pose_cov_23;
889 c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
890 R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
891 R13 * (C13 * R11 + C23 * R12 + C33 * R13);
892 c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
893 (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
894 (C13 * R11 + C23 * R12 + C33 * R13) * R23;
895 c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
896 (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
897 (C13 * R11 + C23 * R12 + C33 * R13) * R33;
898 c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
899 R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
900 R23 * (C13 * R21 + C23 * R22 + C33 * R23);
901 c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
902 (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
903 (C13 * R21 + C23 * R22 + C33 * R23) * R33;
904 c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
905 R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
906 R33 * (C13 * R31 + C23 * R32 + C33 * R33);
918 float Nx = lm->normal.x;
919 float Ny = lm->normal.y;
920 float Nz = lm->normal.z;
922 newLandmark.
normal.
x = Nx * R11 + Ny * R12 + Nz * R13;
923 newLandmark.
normal.
y = Nx * R21 + Ny * R22 + Nz * R23;
924 newLandmark.
normal.
z = Nx * R31 + Ny * R32 + Nz * R33;
926 newLandmark.
ID = lm->ID;
928 newLandmark.
features = lm->features;
933 landmarks.push_back(newLandmark);
940 void CLandmarksMap::fuseWith(
CLandmarksMap& other,
bool justInsertAllOfThem)
946 std::vector<bool> otherCorrs(other.
size(),
false);
948 TMatchingPairList::iterator corrIt;
954 unsigned int nRemoved = 0;
956 if (!justInsertAllOfThem)
960 computeMatchingWith3DLandmarks(&other, corrs, corrsRatio, otherCorrs);
964 for (corrIt = corrs.begin(); corrIt != corrs.end(); corrIt++)
966 thisLM = landmarks.get(corrIt->this_idx);
977 landmarks.isToBeModified(corrIt->this_idx);
984 landmarks.hasBeenModified(corrIt->this_idx);
992 for (i = 0; i < n; i++)
1005 if (!justInsertAllOfThem)
1010 n = landmarks.size();
1011 for (i = n - 1; i >= 0; i--)
1013 if (landmarks.get(i)->getType() !=
1018 std::chrono::duration_cast<std::chrono::milliseconds>(
1019 lastestTime - landmarks.get(i)->timestampLastSeen)
1021 if (dt > fuseOptions.ellapsedTime &&
1022 landmarks.get(i)->seenTimesCount < fuseOptions.minTimesSeen)
1034 "[CLandmarksMap::fuseWith] %u fused/ %u new/ %u removed -> %u "
1036 static_cast<unsigned int>(corrs.size()),
1037 static_cast<unsigned int>(other.
size() - corrs.size()),
1038 static_cast<unsigned int>(nRemoved),
1039 static_cast<unsigned int>(landmarks.size()));
1042 f,
"%u\t%u\t%u\t%u\n",
static_cast<unsigned int>(corrs.size()),
1043 static_cast<unsigned int>(other.
size() - corrs.size()),
1044 static_cast<unsigned int>(nRemoved),
1045 static_cast<unsigned int>(landmarks.size()));
1055 void CLandmarksMap::computeMatchingWith3DLandmarks(
1058 std::vector<bool>& otherCorrespondences)
const
1062 TSequenceLandmarks::const_iterator thisIt, otherIt;
1063 unsigned int nThis, nOther;
1066 unsigned int i, n, j, k;
1068 double lik_dist, lik_desc, lik, maxLik;
1072 std::vector<bool> thisLandmarkAssigned;
1073 double K_desc = 0.0;
1074 double K_dist = 0.0;
1080 nThis = landmarks.size();
1084 thisLandmarkAssigned.resize(nThis,
false);
1087 correspondences.clear();
1088 otherCorrespondences.clear();
1089 otherCorrespondences.resize(nOther,
false);
1090 correspondencesRatio = 0;
1096 switch (insertionOptions.SIFTMatching3DMethod)
1104 -0.5 /
square(likelihoodOptions.SIFTs_sigma_descriptor_dist);
1105 K_dist = -0.5 /
square(likelihoodOptions.SIFTs_mahaDist_std);
1112 otherIt != anotherMap->
landmarks.
end(); otherIt++, k++)
1115 otherIt->getPose(pointPDF_k);
1117 if (otherIt->getType() ==
featSIFT)
1123 for (j = 0, thisIt = landmarks.begin();
1124 thisIt != landmarks.end(); thisIt++, j++)
1126 if (thisIt->getType() ==
featSIFT &&
1127 thisIt->features.size() ==
1128 otherIt->features.size() &&
1129 !thisIt->features.empty() &&
1130 thisIt->features[0].descriptors.SIFT->size() ==
1131 otherIt->features[0].descriptors.SIFT->size())
1137 thisIt->getPose(pointPDF_j);
1144 double distMahaFlik2;
1152 pointPDF_k.
mean.z() - pointPDF_j.
mean.z();
1162 lik_dist = exp(K_dist * distMahaFlik2);
1165 if (lik_dist > 1e-2)
1178 mPair(thisIt->ID, otherIt->ID);
1180 if (CLandmarksMap::_mEDD[mPair] == 0)
1182 n = otherIt->features[0]
1183 .descriptors.SIFT->size();
1185 for (i = 0; i < n; i++)
1187 (*otherIt->features[0]
1188 .descriptors.SIFT)[i] -
1189 (*thisIt->features[0]
1190 .descriptors.SIFT)[i]);
1192 CLandmarksMap::_mEDD[mPair] = desc;
1196 desc = CLandmarksMap::_mEDD[mPair];
1199 lik_desc = exp(K_desc * desc);
1210 lik = lik_dist * lik_desc;
1233 if (maxLik > insertionOptions.SiftLikelihoodThreshold)
1237 if (!thisLandmarkAssigned[maxIdx])
1239 thisLandmarkAssigned[maxIdx] =
true;
1242 otherCorrespondences[k] =
true;
1245 match.
this_x = landmarks.get(maxIdx)->pose_mean.x;
1246 match.
this_y = landmarks.get(maxIdx)->pose_mean.y;
1247 match.
this_z = landmarks.get(maxIdx)->pose_mean.z;
1257 correspondences.push_back(match);
1266 correspondencesRatio = correspondences.size() /
d2f(nOther);
1283 ASSERT_(!landmarks.begin()->features.empty());
1286 .descriptors.SIFT->size();
1288 otherIt != anotherMap->
landmarks.
end(); otherIt++, k++)
1291 unsigned int mEDDidx = 0;
1292 for (j = 0, thisIt = landmarks.begin();
1293 thisIt != landmarks.end(); thisIt++, j++)
1297 for (i = 0; i < dLen; i++)
1299 (*otherIt->features[0].descriptors.SIFT)[i] -
1300 (*thisIt->features[0].descriptors.SIFT)[i]);
1311 if (mEDD > insertionOptions.SiftEDDThreshold)
1314 if (!thisLandmarkAssigned[mEDDidx])
1318 thisLandmarkAssigned[mEDDidx] =
true;
1321 otherCorrespondences[k] =
true;
1323 otherIt->getPose(pointPDF_k);
1324 thisIt->getPose(pointPDF_j);
1327 match.
this_x = landmarks.get(mEDDidx)->pose_mean.x;
1328 match.
this_y = landmarks.get(mEDDidx)->pose_mean.y;
1329 match.
this_z = landmarks.get(mEDDidx)->pose_mean.z;
1339 correspondences.push_back(match);
1347 correspondencesRatio = correspondences.size() /
d2f(nOther);
1359 bool CLandmarksMap::saveToTextFile(std::string file)
1363 FILE* f =
os::fopen(file.c_str(),
"wt");
1364 if (!f)
return false;
1373 for (
auto it = landmarks.begin(); it != landmarks.end(); ++it)
1376 f,
"%10f %10f %10f %4i %4u %10f", it->pose_mean.x, it->pose_mean.y,
1377 it->pose_mean.z,
static_cast<int>(it->getType()),
1382 it->timestampLastSeen));
1386 ASSERT_(!it->features.empty());
1387 for (
unsigned char i : *it->features[0].descriptors.SIFT)
1405 bool CLandmarksMap::saveToMATLABScript3D(
1406 std::string file,
const char* style,
float confInterval)
const
1408 FILE* f =
os::fopen(file.c_str(),
"wt");
1409 if (!f)
return false;
1413 f,
"%%-------------------------------------------------------\n");
1414 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1415 os::fprintf(f,
"%% 'CLandmarksMap::saveToMATLABScript3D'\n");
1419 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1422 f,
"%%-------------------------------------------------------\n\n");
1427 for (
const auto& landmark : landmarks)
1430 f,
"m=[%.4f %.4f %.4f];", landmark.pose_mean.x,
1431 landmark.pose_mean.y, landmark.pose_mean.z);
1433 f,
"c=[%.8f %.8f %.8f;%.8f %.8f %.8f;%.8f %.8f %.8f]; ",
1434 landmark.pose_cov_11, landmark.pose_cov_12, landmark.pose_cov_13,
1435 landmark.pose_cov_12, landmark.pose_cov_22, landmark.pose_cov_23,
1436 landmark.pose_cov_13, landmark.pose_cov_23, landmark.pose_cov_33);
1440 "error_ellipse(c,m,'conf',%f,'style','%s','numPointsEllipse',10);"
1442 confInterval, style);
1445 os::fprintf(f,
"axis equal;grid on;xlabel('x'); ylabel('y'); zlabel('z');");
1455 bool CLandmarksMap::saveToMATLABScript2D(
1456 std::string file,
const char* style,
float stdCount)
1458 FILE* f =
os::fopen(file.c_str(),
"wt");
1459 if (!f)
return false;
1461 const int ELLIPSE_POINTS = 30;
1462 std::vector<float> X, Y, COS, SIN;
1463 std::vector<float>::iterator x, y, Cos, Sin;
1467 X.resize(ELLIPSE_POINTS);
1468 Y.resize(ELLIPSE_POINTS);
1469 COS.resize(ELLIPSE_POINTS);
1470 SIN.resize(ELLIPSE_POINTS);
1473 for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
1474 Cos++, Sin++, ang += (
M_2PI / (ELLIPSE_POINTS - 1)))
1482 f,
"%%-------------------------------------------------------\n");
1483 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1484 os::fprintf(f,
"%% 'CLandmarksMap::saveToMATLABScript2D'\n");
1488 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1491 f,
"%%-------------------------------------------------------\n\n");
1496 for (
auto& landmark : landmarks)
1499 cov(0, 0) = landmark.pose_cov_11;
1500 cov(1, 1) = landmark.pose_cov_22;
1501 cov(0, 1) =
cov(1, 0) = landmark.pose_cov_12;
1503 std::vector<double> eigvals;
1507 eigVal = eigVal.
array().sqrt().matrix();
1512 for (x = X.begin(), y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin();
1513 x != X.end(); x++, y++, Cos++, Sin++)
1516 (landmark.pose_mean.x +
1517 stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
1519 (landmark.pose_mean.y +
1520 stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
1526 for (x = X.begin(); x != X.end(); x++)
1532 for (y = Y.begin(); y != Y.end(); y++)
1551 void CLandmarksMap::loadOccupancyFeaturesFrom2DRangeScan(
1553 unsigned int downSampleFactor)
1557 double J11, J12, J21, J22;
1568 sensorGlobalPose = *robotPose + obs.
sensorPose;
1583 double var_d =
square(0.005);
1584 double var_th =
square(dTh / 10.0);
1587 for (i = 0; i < n; i += downSampleFactor, Th += downSampleFactor * dTh)
1606 newLandmark.
features[0].orientation = Th;
1607 newLandmark.
features[0].keypoint.octave = d;
1625 newLandmark.
pose_cov_11 = (J11 * J11 * var_th + J12 * J12 * var_d);
1626 newLandmark.
pose_cov_12 = (J11 * J21 * var_th + J12 * J22 * var_d);
1627 newLandmark.
pose_cov_22 = (J21 * J21 * var_th + J22 * J22 * var_d);
1633 landmarks.push_back(newLandmark);
1641 changeCoordinatesReference(sensorGlobalPose);
1651 double CLandmarksMap::computeLikelihood_RSLC_2007(
1657 TSequenceLandmarks::const_iterator itOther;
1664 double nFoundCorrs = 0;
1665 std::vector<int32_t>* corrs;
1666 unsigned int cx, cy, cx_1, cx_2, cy_1, cy_2;
1678 itOther->getPose(poseOther);
1680 cx = cy = grid->
y2idx(itOther->pose_mean.y);
1682 cx_1 = max(0, grid->
x2idx(itOther->pose_mean.x - 0.10f));
1684 min(
static_cast<int>(grid->
getSizeX()) - 1,
1685 grid->
x2idx(itOther->pose_mean.x + 0.10f));
1686 cy_1 = max(0, grid->
y2idx(itOther->pose_mean.y - 0.10f));
1688 min(
static_cast<int>(grid->
getSizeY()) - 1,
1689 grid->
y2idx(itOther->pose_mean.y + 0.10f));
1700 for (cx = cx_1; cx <= cx_2; cx++)
1701 for (cy = cy_1; cy <= cy_2; cy++)
1705 if (!corrs->empty())
1706 for (
int& it : *corrs)
1708 lm = landmarks.get(it);
1714 if (fabs(lm->
pose_mean.
x - itOther->pose_mean.x) >
1716 fabs(lm->
pose_mean.
y - itOther->pose_mean.y) >
1733 PrNoCorr *= 1 - corr;
1740 nFoundCorrs += 1 - PrNoCorr;
1771 lik *= 1 - PrNoCorr;
1775 lik = nFoundCorrs /
static_cast<double>(s->
size());
1790 CLandmarksMap::TCustomSequenceLandmarks::TCustomSequenceLandmarks()
1791 : m_landmarks(), m_grid(-10.0f, 10.0f, -10.0f, 10.f, 0.20f)
1798 m_landmarks.clear();
1803 m_largestDistanceFromOriginIsUpdated =
false;
1809 std::vector<int32_t> dummyEmpty;
1815 max(m_grid.getYMax(), l.
pose_mean.
y + 0.1), dummyEmpty);
1817 m_landmarks.push_back(l);
1822 cell->push_back(m_landmarks.size() - 1);
1824 m_largestDistanceFromOriginIsUpdated =
false;
1829 return &m_landmarks[indx];
1833 unsigned int indx)
const
1835 return &m_landmarks[indx];
1840 std::vector<int32_t>* cell = m_grid.cellByPos(
1841 m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1843 std::vector<int32_t>::iterator it;
1844 for (it = cell->begin(); it != cell->end(); it++)
1846 if (*it ==
static_cast<int>(indx))
1853 m_largestDistanceFromOriginIsUpdated =
false;
1858 m_landmarks.erase(m_landmarks.begin() + indx);
1859 m_largestDistanceFromOriginIsUpdated =
false;
1864 std::vector<int32_t> dummyEmpty;
1868 min(m_grid.getXMin(), m_landmarks[indx].pose_mean.x),
1869 max(m_grid.getXMax(), m_landmarks[indx].pose_mean.x),
1870 min(m_grid.getYMin(), m_landmarks[indx].pose_mean.y),
1871 max(m_grid.getYMax(), m_landmarks[indx].pose_mean.y), dummyEmpty);
1874 std::vector<int32_t>* cell = m_grid.cellByPos(
1875 m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1876 cell->push_back(indx);
1877 m_largestDistanceFromOriginIsUpdated =
false;
1884 TSequenceLandmarks::iterator it;
1886 double min_x = -10.0, max_x = 10.0;
1887 double min_y = -10.0, max_y = 10.0;
1888 std::vector<int32_t> dummyEmpty;
1894 for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1897 min_x = min(min_x, it->pose_mean.x);
1898 max_x = max(max_x, it->pose_mean.x);
1899 min_y = min(min_y, it->pose_mean.y);
1900 max_y = max(max_y, it->pose_mean.y);
1902 m_grid.resize(min_x, max_x, min_y, max_y, dummyEmpty);
1905 for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1908 std::vector<int32_t>* cell =
1909 m_grid.cellByPos(it->pose_mean.x, it->pose_mean.y);
1910 cell->push_back(idx);
1913 m_largestDistanceFromOriginIsUpdated =
false;
1924 if (!m_largestDistanceFromOriginIsUpdated)
1927 float maxDistSq = 0, d;
1928 for (
const auto& it : *
this)
1932 maxDistSq = max(d, maxDistSq);
1935 m_largestDistanceFromOrigin = sqrt(maxDistSq);
1936 m_largestDistanceFromOriginIsUpdated =
true;
1938 return m_largestDistanceFromOrigin;
1946 std::vector<bool>* otherCorrespondences)
1950 unsigned long distDesc;
1951 double likByDist, likByDesc;
1953 std::vector<unsigned char>::iterator it1, it2;
1958 unsigned int idx1, idx2;
1960 CMatrixD dij(1, 3), Cij(3, 3), Cij_1;
1961 double distMahaFlik2;
1972 TSequenceLandmarks::iterator lm1, lm2;
1975 lm1 += decimation, idx1 += decimation)
1980 lm1->getPose(lm1_pose);
1991 lm2->getPose(lm2_pose);
1996 dij(0, 0) = lm1->pose_mean.x - lm2->pose_mean.x;
1997 dij(0, 1) = lm1->pose_mean.y - lm2->pose_mean.y;
1998 dij(0, 2) = lm1->pose_mean.z - lm2->pose_mean.z;
2010 likByDist = exp(K_dist * distMahaFlik2);
2012 if (likByDist > 1e-2)
2024 mPair(lm2->ID, lm1->ID);
2032 !lm1->features.empty() &&
2033 !lm2->features.empty());
2036 .descriptors.SIFT->size() ==
2038 .descriptors.SIFT->size());
2040 for (it1 = lm1->features[0]
2041 .descriptors.SIFT->begin(),
2042 it2 = lm2->features[0]
2043 .descriptors.SIFT->begin();
2044 it1 != lm1->features[0]
2045 .descriptors.SIFT->end();
2047 distDesc +=
square(*it1 - *it2);
2055 distDesc = (
unsigned long)
2058 likByDesc = exp(K_desc * distDesc);
2059 lik_i += likByDist *
2071 lik *= (0.1 + 0.9 * lik_i);
2079 TMatchingPairList::iterator itCorr;
2084 if (correspondences ==
nullptr)
2086 "When using this method with SIFTLikelihoodMethod = 1, "
2087 "TMatchingPairList *correspondence can not be NULL");
2089 if (otherCorrespondences ==
nullptr)
2091 "When using this method with SIFTLikelihoodMethod = 1, "
2092 "std::vector<bool> *otherCorrespondences can not be NULL");
2094 for (itCorr = correspondences->begin();
2095 itCorr != correspondences->end(); itCorr++)
2114 lik *= exp(-0.5 * dist);
2119 for (
size_t k = 1; k <= (theMap->
size() - correspondences->size());
2138 SIFT_feat_options(vision::
featSIFT)
2147 out <<
"\n----------- [CLandmarksMap::TInsertionOptions] ------------ \n\n";
2150 "insert_SIFTs_from_monocular_images = %c\n",
2151 insert_SIFTs_from_monocular_images ?
'Y' :
'N');
2153 "insert_SIFTs_from_stereo_images = %c\n",
2154 insert_SIFTs_from_stereo_images ?
'Y' :
'N');
2156 "insert_Landmarks_from_range_scans = %c\n",
2157 insert_Landmarks_from_range_scans ?
'Y' :
'N');
2160 "SiftCorrRatioThreshold = %f\n",
2161 SiftCorrRatioThreshold);
2163 "SiftLikelihoodThreshold = %f\n",
2164 SiftLikelihoodThreshold);
2166 "SiftEDDThreshold = %f\n", SiftEDDThreshold);
2168 "SIFTMatching3DMethod = %d\n", SIFTMatching3DMethod);
2170 "SIFTLikelihoodMethod = %d\n", SIFTLikelihoodMethod);
2173 "SIFTsLoadDistanceOfTheMean = %f\n",
2174 SIFTsLoadDistanceOfTheMean);
2176 "SIFTsLoadEllipsoidWidth = %f\n",
2177 SIFTsLoadEllipsoidWidth);
2180 "SIFTs_stdXY = %f\n", SIFTs_stdXY);
2182 "SIFTs_stdDisparity = %f\n", SIFTs_stdDisparity);
2185 "SIFTs_numberOfKLTKeypoints = %i\n",
2186 SIFTs_numberOfKLTKeypoints);
2188 "SIFTs_stereo_maxDepth = %f\n",
2189 SIFTs_stereo_maxDepth);
2191 "SIFTs_epipolar_TH = %f\n", SIFTs_epipolar_TH);
2193 "PLOT_IMAGES = %c\n",
2194 PLOT_IMAGES ?
'Y' :
'N');
2196 SIFT_feat_options.dumpToTextStream(
out);
2207 insert_SIFTs_from_monocular_images =
iniFile.read_bool(
2208 section.c_str(),
"insert_SIFTs_from_monocular_images",
2209 insert_SIFTs_from_monocular_images);
2210 insert_SIFTs_from_stereo_images =
iniFile.read_bool(
2211 section.c_str(),
"insert_SIFTs_from_stereo_images",
2212 insert_SIFTs_from_stereo_images);
2213 insert_Landmarks_from_range_scans =
iniFile.read_bool(
2214 section.c_str(),
"insert_Landmarks_from_range_scans",
2215 insert_Landmarks_from_range_scans);
2216 SiftCorrRatioThreshold =
iniFile.read_float(
2217 section.c_str(),
"SiftCorrRatioThreshold", SiftCorrRatioThreshold);
2218 SiftLikelihoodThreshold =
iniFile.read_float(
2219 section.c_str(),
"SiftLikelihoodThreshold", SiftLikelihoodThreshold);
2220 SiftEDDThreshold =
iniFile.read_float(
2221 section.c_str(),
"SiftEDDThreshold", SiftEDDThreshold);
2222 SIFTMatching3DMethod =
iniFile.read_int(
2223 section.c_str(),
"SIFTMatching3DMethod", SIFTMatching3DMethod);
2224 SIFTLikelihoodMethod =
iniFile.read_int(
2225 section.c_str(),
"SIFTLikelihoodMethod", SIFTLikelihoodMethod);
2226 SIFTsLoadDistanceOfTheMean =
iniFile.read_float(
2227 section.c_str(),
"SIFTsLoadDistanceOfTheMean",
2228 SIFTsLoadDistanceOfTheMean);
2229 SIFTsLoadEllipsoidWidth =
iniFile.read_float(
2230 section.c_str(),
"SIFTsLoadEllipsoidWidth", SIFTsLoadEllipsoidWidth);
2232 iniFile.read_float(section.c_str(),
"SIFTs_stdXY", SIFTs_stdXY);
2233 SIFTs_stdDisparity =
iniFile.read_float(
2234 section.c_str(),
"SIFTs_stdDisparity", SIFTs_stdDisparity);
2235 SIFTs_numberOfKLTKeypoints =
iniFile.read_int(
2236 section.c_str(),
"SIFTs_numberOfKLTKeypoints",
2237 SIFTs_numberOfKLTKeypoints);
2238 SIFTs_stereo_maxDepth =
iniFile.read_float(
2239 section.c_str(),
"SIFTs_stereo_maxDepth", SIFTs_stereo_maxDepth);
2240 SIFTs_epipolar_TH =
iniFile.read_float(
2241 section.c_str(),
"SIFTs_epipolar_TH", SIFTs_epipolar_TH);
2243 iniFile.read_bool(section.c_str(),
"PLOT_IMAGES", PLOT_IMAGES);
2245 SIFT_feat_options.loadFromConfigFile(
iniFile, section);
2252 : SIFT_feat_options(vision::
featSIFT),
2267 std::ostream&
out)
const
2269 out <<
"\n----------- [CLandmarksMap::TLikelihoodOptions] ------------ "
2273 "rangeScan2D_decimation = %i\n",
2274 rangeScan2D_decimation);
2276 "SIFTs_sigma_euclidean_dist = %f\n",
2277 SIFTs_sigma_euclidean_dist);
2279 "SIFTs_sigma_descriptor_dist = %f\n",
2280 SIFTs_sigma_descriptor_dist);
2282 "SIFTs_mahaDist_std = %f\n", SIFTs_mahaDist_std);
2284 "SIFTs_decimation = %i\n", SIFTs_decimation);
2286 "SIFTnullCorrespondenceDistance = %f\n",
2287 SIFTnullCorrespondenceDistance);
2289 "beaconRangesStd = %f\n", beaconRangesStd);
2291 "beaconRangesUseObservationStd = %c\n",
2292 beaconRangesUseObservationStd ?
'Y' :
'N');
2294 "extRobotPoseStd = %f\n", extRobotPoseStd);
2297 "GPSOrigin:LATITUDE = %f\n", GPSOrigin.latitude);
2299 "GPSOrigin:LONGITUDE = %f\n", GPSOrigin.longitude);
2301 "GPSOrigin:ALTITUDE = %f\n", GPSOrigin.altitude);
2303 "GPSOrigin:Rotation_Angle = %f\n", GPSOrigin.ang);
2305 "GPSOrigin:x_shift = %f\n", GPSOrigin.x_shift);
2307 "GPSOrigin:y_shift = %f\n", GPSOrigin.y_shift);
2309 "GPSOrigin:min_sat = %i\n", GPSOrigin.min_sat);
2312 "GPS_sigma = %f (m)\n", GPS_sigma);
2314 SIFT_feat_options.dumpToTextStream(
out);
2325 rangeScan2D_decimation =
iniFile.read_int(
2326 section.c_str(),
"rangeScan2D_decimation", rangeScan2D_decimation);
2327 SIFTs_sigma_euclidean_dist =
iniFile.read_double(
2328 section.c_str(),
"SIFTs_sigma_euclidean_dist",
2329 SIFTs_sigma_euclidean_dist);
2330 SIFTs_sigma_descriptor_dist =
iniFile.read_double(
2331 section.c_str(),
"SIFTs_sigma_descriptor_dist",
2332 SIFTs_sigma_descriptor_dist);
2333 SIFTs_mahaDist_std =
iniFile.read_float(
2334 section.c_str(),
"SIFTs_mahaDist_std", SIFTs_mahaDist_std);
2336 iniFile.read_int(section.c_str(),
"SIFTs_decimation", SIFTs_decimation);
2337 SIFTnullCorrespondenceDistance =
iniFile.read_float(
2338 section.c_str(),
"SIFTnullCorrespondenceDistance",
2339 SIFTnullCorrespondenceDistance);
2341 GPSOrigin.latitude =
iniFile.read_double(
2342 section.c_str(),
"GPSOriginLatitude", GPSOrigin.latitude);
2343 GPSOrigin.longitude =
iniFile.read_double(
2344 section.c_str(),
"GPSOriginLongitude", GPSOrigin.longitude);
2345 GPSOrigin.altitude =
iniFile.read_double(
2346 section.c_str(),
"GPSOriginAltitude", GPSOrigin.altitude);
2348 iniFile.read_double(section.c_str(),
"GPSOriginAngle", GPSOrigin.ang) *
2350 GPSOrigin.x_shift =
iniFile.read_double(
2351 section.c_str(),
"GPSOriginXshift", GPSOrigin.x_shift);
2352 GPSOrigin.y_shift =
iniFile.read_double(
2353 section.c_str(),
"GPSOriginYshift", GPSOrigin.y_shift);
2355 iniFile.read_int(section.c_str(),
"GPSOriginMinSat", GPSOrigin.min_sat);
2357 GPS_sigma =
iniFile.read_float(section.c_str(),
"GPSSigma", GPS_sigma);
2360 iniFile.read_float(section.c_str(),
"beaconRangesStd", beaconRangesStd);
2361 beaconRangesUseObservationStd =
iniFile.read_bool(
2362 section.c_str(),
"beaconRangesUseObservationStd",
2363 beaconRangesUseObservationStd);
2366 iniFile.read_float(section.c_str(),
"extRobotPoseStd", extRobotPoseStd);
2368 SIFT_feat_options.loadFromConfigFile(
iniFile, section);
2386 const CPose3D& in_robotPose,
const CPoint3D& in_sensorLocationOnRobot,
2389 TSequenceLandmarks::const_iterator it;
2395 point3D = in_robotPose + in_sensorLocationOnRobot;
2407 it->getPose(beaconPDF);
2408 beacon3D = beaconPDF.
mean;
2413 range += out_Observations.
stdError *
2415 range = max(0.0f, range);
2426 out_Observations.
sensedData.push_back(newMeas);
2437 const std::string& filNamePrefix)
const
2442 std::string fil1(filNamePrefix + std::string(
"_3D.m"));
2452 std::make_shared<opengl::CGridPlaneXY>(-100, 100, -100, 100, 0, 1);
2457 std::string fil2(filNamePrefix + std::string(
"_3D.3Dscene"));
2478 std::make_shared<opengl::CEllipsoid3D>();
2480 landmark.getPose(pointGauss);
2482 ellip->setPose(pointGauss.
mean);
2483 ellip->setCovMatrix(pointGauss.
cov);
2484 ellip->enableDrawSolid3D(
false);
2485 ellip->setQuantiles(3.0);
2486 ellip->set3DsegmentsCount(10);
2487 ellip->setColor(0, 0, 1);
2489 mrpt::format(
"LM.ID=%u",
static_cast<unsigned int>(landmark.ID)));
2490 ellip->enableShowName(
true);
2492 outObj->insert(ellip);
2505 for (
const auto& m_landmark : m_landmarks)
2507 if (m_landmark.ID == ID)
return &m_landmark;
2524 unsigned int ID)
const
2526 for (
const auto& m_landmark : m_landmarks)
2528 if (m_landmark.ID == ID)
return &m_landmark;
2563 if (!otherMap)
return 0;
2566 std::deque<CPointPDFGaussian::Ptr> poses3DThis, poses3DOther;
2567 std::deque<CPointPDFGaussian::Ptr>::iterator itPoseThis, itPoseOther;
2570 size_t nOther = otherMap->landmarks.size();
2571 size_t otherLandmarkWithCorrespondence = 0;
2574 if (!nThis)
return 0;
2575 if (!nOther)
return 0;
2580 float Tx = pose3DMatrix(0, 3);
2581 float Ty = pose3DMatrix(1, 3);
2582 float Tz = pose3DMatrix(2, 3);
2592 otherMap->landmarks.getLargestDistanceFromOrigin() + 1.0f))
2596 poses3DOther.resize(nOther);
2597 for (
size_t i = 0; i < nOther; i++)
2598 poses3DOther[i] = std::make_shared<CPointPDFGaussian>();
2600 poses3DThis.resize(nThis);
2601 for (
size_t i = 0; i < nThis; i++)
2602 poses3DThis[i] = std::make_shared<CPointPDFGaussian>();
2605 for (itOther = otherMap->landmarks.begin(),
2606 itPoseOther = poses3DOther.begin();
2607 itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2609 itOther->getPose(**itPoseOther);
2610 (*itPoseOther)->changeCoordinatesReference(otherMapPose);
2614 for (itThis =
landmarks.
begin(), itPoseThis = poses3DThis.begin();
2617 itThis->getPose(**itPoseThis);
2621 for (itOther = otherMap->landmarks.begin(),
2622 itPoseOther = poses3DOther.begin();
2623 itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2625 for (itThis =
landmarks.
begin(), itPoseThis = poses3DThis.begin();
2631 (*itPoseThis)->mean.x() - (*itPoseOther)->mean.x(),
2632 (*itPoseThis)->mean.y() - (*itPoseOther)->mean.y(),
2633 (*itPoseThis)->mean.z() - (*itPoseOther)->mean.z());
2642 if (distMaha <
params.maxMahaDistForCorr)
2645 if (!itThis->features.empty() && !itOther->features.empty() &&
2646 itThis->features[0].descriptors.SIFT->size() ==
2647 itOther->features[0].descriptors.SIFT->size())
2649 unsigned long descrDist = 0;
2650 std::vector<unsigned char>::const_iterator it1, it2;
2651 for (it1 = itThis->features[0].descriptors.SIFT->begin(),
2652 it2 = itOther->features[0].descriptors.SIFT->begin();
2653 it1 != itThis->features[0].descriptors.SIFT->end();
2655 descrDist +=
square(*it1 - *it2);
2658 sqrt(
d2f(descrDist)) /
2659 itThis->features[0].descriptors.SIFT->size();
2661 if (descrDist_f < 1.5f)
2663 otherLandmarkWithCorrespondence++;
2672 return d2f(otherLandmarkWithCorrespondence) / nOther;
2690 const CPose3D& in_robotPose,
const CPose3D& in_sensorLocationOnRobot,
2692 const float in_stdRange,
const float in_stdYaw,
const float in_stdPitch,
2693 std::vector<size_t>* out_real_associations,
2694 const double spurious_count_mean,
const double spurious_count_std)
const
2696 TSequenceLandmarks::const_iterator it;
2702 if (out_real_associations) out_real_associations->clear();
2705 const CPose3D point3D = in_robotPose +
CPose3D(in_sensorLocationOnRobot);
2722 it->getPose(beaconPDF);
2726 double range, yaw,
pitch;
2735 range = max(0.0, range);
2743 if (sensorDetectsIDs)
2747 newMeas.
range = range;
2752 out_Observations.
sensedData.push_back(newMeas);
2754 if (out_real_associations)
2755 out_real_associations->push_back(idx);
2760 spurious_count_mean, spurious_count_std);
2761 size_t nSpurious = 0;
2762 if (spurious_count_std != 0 || spurious_count_mean != 0)
2768 for (
size_t i = 0; i < nSpurious; i++)
2781 const double pitch =
2791 newMeas.
range = range;
2796 out_Observations.
sensedData.push_back(newMeas);
2798 if (out_real_associations)
2799 out_real_associations->push_back(