49 void CLandmarksMap::TMapDefinition::loadFromConfigFile_map_specific(
55 sectionNamePrefix +
string(
"_creationOpts");
56 this->initialBeacons.clear();
57 const unsigned int nBeacons =
source.read_int(sSectCreation,
"nBeacons", 0);
58 for (
unsigned int q = 1;
q <= nBeacons;
q++)
62 source.read_int(sSectCreation,
format(
"beacon_%03u_ID",
q), 0);
65 source.read_float(sSectCreation,
format(
"beacon_%03u_x",
q), 0);
67 source.read_float(sSectCreation,
format(
"beacon_%03u_y",
q), 0);
69 source.read_float(sSectCreation,
format(
"beacon_%03u_z",
q), 0);
71 this->initialBeacons.push_back(newPair);
74 insertionOpts.loadFromConfigFile(
75 source, sectionNamePrefix +
string(
"_insertOpts"));
76 likelihoodOpts.loadFromConfigFile(
77 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
80 void CLandmarksMap::TMapDefinition::dumpToTextStream_map_specific(
84 "number of initial beacons = %u\n",
85 (
int)initialBeacons.size());
87 out.
printf(
" ID (X,Y,Z)\n");
88 out.
printf(
"--------------------------------------------------------\n");
90 p != initialBeacons.end(); ++
p)
92 " %03u (%8.03f,%8.03f,%8.03f)\n",
p->second,
93 p->first.x,
p->first.y,
p->first.z);
95 this->insertionOpts.dumpToTextStream(out);
96 this->likelihoodOpts.dumpToTextStream(out);
124 obj->landmarks.push_back(lm);
141 CLandmarksMap::_mEDD;
143 bool CLandmarksMap::_maxIDUpdated =
false;
147 CLandmarksMap::CLandmarksMap()
219 for (i = 0; i <
n; i++)
305 unsigned int sensedID = it->beaconID;
312 (lm_it->ID == sensedID) &&
313 (!std::isnan(it->sensedDistance)))
315 lm_it->getPose(beaconPDF);
316 beacon3D = beaconPDF.
mean;
319 point3D = robotPose3D + it->sensorLocationOnRobot;
321 const float expectedRange = point3D.
distanceTo(beacon3D);
322 float sensedDist = it->sensedDistance;
323 if (sensedDist < 0) sensedDist = 0;
331 square((expectedRange - sensedDist) / sensorStd));
367 CMatrixD dij(1, 6), Cij(6, 6), Cij_1;
368 dij(0, 0) = o->
pose.
mean.
x() - sensorPose3D.
x();
369 dij(0, 1) = o->
pose.
mean.
y() - sensorPose3D.
y();
370 dij(0, 2) = o->
pose.
mean.z() - sensorPose3D.z();
379 double distMahaFlik2 = dij.multiply_HCHt_scalar(Cij_1);
399 double earth_radius = 6378137;
489 robotPose2D =
CPose2D(*robotPose);
490 robotPose3D = (*robotPose);
606 float maxDistForCorrespondence,
float maxAngularDistForCorrespondence,
608 float& correspondencesRatio,
float* sumSqrDist,
bool onlyKeepTheClosest,
609 bool onlyUniqueRobust)
const 621 CPose3D otherMapPose3D(otherMapPose);
623 correspondencesRatio = 0;
629 std::vector<bool> otherCorrespondences;
636 otherMap2, correspondences, correspondencesRatio, otherCorrespondences);
676 for (sift = siftList.
begin(); sift != siftList.
end(); sift++)
690 landmark3DPositionPDF.
mean *= d;
696 D(0, 0) =
square(0.5 * d);
709 Normal *= -1 / Normal.
norm();
745 fExt.options = feat_options;
763 leftSiftList, rightSiftList, matchesList, matchingOptions);
767 std::cerr <<
"Warning: insertionOptions.PLOT_IMAGES has no effect " 768 "since MRPT 0.9.1\n";
786 stereoParams.
minZ = 0.0f;
789 size_t numM = matchesList.size();
797 (*ii).seenTimesCount = 1;
801 "%u (out of %u) corrs!\n", static_cast<unsigned>(
landmarks.
size()),
802 static_cast<unsigned>(numM));
823 double R11 = HM.get_unsafe(0, 0);
824 double R12 = HM.get_unsafe(0, 1);
825 double R13 = HM.get_unsafe(0, 2);
826 double R21 = HM.get_unsafe(1, 0);
827 double R22 = HM.get_unsafe(1, 1);
828 double R23 = HM.get_unsafe(1, 2);
829 double R31 = HM.get_unsafe(2, 0);
830 double R32 = HM.get_unsafe(2, 1);
831 double R33 = HM.get_unsafe(2, 2);
833 double c11, c22, c33, c12, c13, c23;
843 float C11 = lm->pose_cov_11;
844 float C22 = lm->pose_cov_22;
845 float C33 = lm->pose_cov_33;
846 float C12 = lm->pose_cov_12;
847 float C13 = lm->pose_cov_13;
848 float C23 = lm->pose_cov_23;
851 c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
852 R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
853 R13 * (C13 * R11 + C23 * R12 + C33 * R13);
854 c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
855 (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
856 (C13 * R11 + C23 * R12 + C33 * R13) * R23;
857 c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
858 (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
859 (C13 * R11 + C23 * R12 + C33 * R13) * R33;
860 c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
861 R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
862 R23 * (C13 * R21 + C23 * R22 + C33 * R23);
863 c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
864 (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
865 (C13 * R21 + C23 * R22 + C33 * R23) * R33;
866 c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
867 R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
868 R33 * (C13 * R31 + C23 * R32 + C33 * R33);
871 lm->pose_cov_11 = c11;
872 lm->pose_cov_22 = c22;
873 lm->pose_cov_33 = c33;
874 lm->pose_cov_12 = c12;
875 lm->pose_cov_13 = c13;
876 lm->pose_cov_23 = c23;
880 float Nx = lm->normal.x;
881 float Ny = lm->normal.y;
882 float Nz = lm->normal.z;
884 lm->normal.x = Nx * R11 + Ny * R12 + Nz * R13;
885 lm->normal.y = Nx * R21 + Ny * R22 + Nz * R23;
886 lm->normal.z = Nx * R31 + Ny * R32 + Nz * R33;
906 double R11 = HM.get_unsafe(0, 0);
907 double R12 = HM.get_unsafe(0, 1);
908 double R13 = HM.get_unsafe(0, 2);
909 double R21 = HM.get_unsafe(1, 0);
910 double R22 = HM.get_unsafe(1, 1);
911 double R23 = HM.get_unsafe(1, 2);
912 double R31 = HM.get_unsafe(2, 0);
913 double R32 = HM.get_unsafe(2, 1);
914 double R33 = HM.get_unsafe(2, 2);
916 double c11, c22, c33, c12, c13, c23;
930 float C11 = lm->pose_cov_11;
931 float C22 = lm->pose_cov_22;
932 float C33 = lm->pose_cov_33;
933 float C12 = lm->pose_cov_12;
934 float C13 = lm->pose_cov_13;
935 float C23 = lm->pose_cov_23;
938 c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
939 R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
940 R13 * (C13 * R11 + C23 * R12 + C33 * R13);
941 c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
942 (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
943 (C13 * R11 + C23 * R12 + C33 * R13) * R23;
944 c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
945 (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
946 (C13 * R11 + C23 * R12 + C33 * R13) * R33;
947 c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
948 R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
949 R23 * (C13 * R21 + C23 * R22 + C33 * R23);
950 c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
951 (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
952 (C13 * R21 + C23 * R22 + C33 * R23) * R33;
953 c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
954 R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
955 R33 * (C13 * R31 + C23 * R32 + C33 * R33);
967 float Nx = lm->normal.x;
968 float Ny = lm->normal.y;
969 float Nz = lm->normal.z;
971 newLandmark.
normal.
x = Nx * R11 + Ny * R12 + Nz * R13;
972 newLandmark.
normal.
y = Nx * R21 + Ny * R22 + Nz * R23;
973 newLandmark.
normal.
z = Nx * R31 + Ny * R32 + Nz * R33;
975 newLandmark.
ID = lm->ID;
977 newLandmark.
features = lm->features;
995 std::vector<bool> otherCorrs(other.
size(),
false);
1001 bool verbose =
true;
1003 unsigned int nRemoved = 0;
1005 if (!justInsertAllOfThem)
1013 for (corrIt = corrs.begin(); corrIt != corrs.end(); corrIt++)
1041 for (i = 0; i <
n; i++)
1054 if (!justInsertAllOfThem)
1060 for (i =
n - 1; i >= 0; i--)
1082 "[CLandmarksMap::fuseWith] %u fused/ %u new/ %u removed -> %u " 1084 static_cast<unsigned int>(corrs.size()),
1085 static_cast<unsigned int>(other.
size() - corrs.size()),
1086 static_cast<unsigned int>(nRemoved),
1090 f,
"%u\t%u\t%u\t%u\n", static_cast<unsigned int>(corrs.size()),
1091 static_cast<unsigned int>(other.
size() - corrs.size()),
1092 static_cast<unsigned int>(nRemoved),
1106 std::vector<bool>& otherCorrespondences)
const 1111 unsigned int nThis, nOther;
1114 unsigned int i,
n, j, k;
1116 double lik_dist, lik_desc, lik, maxLik;
1120 std::vector<bool> thisLandmarkAssigned;
1121 double K_desc = 0.0;
1122 double K_dist = 0.0;
1132 thisLandmarkAssigned.resize(nThis,
false);
1135 correspondences.clear();
1136 otherCorrespondences.clear();
1137 otherCorrespondences.resize(nOther,
false);
1138 correspondencesRatio = 0;
1160 otherIt != anotherMap->
landmarks.
end(); otherIt++, k++)
1163 otherIt->getPose(pointPDF_k);
1165 if (otherIt->getType() ==
featSIFT)
1187 if (thisIt->getType() ==
featSIFT &&
1188 thisIt->features.size() ==
1189 otherIt->features.size() &&
1190 !thisIt->features.empty() && thisIt->features[0] &&
1191 otherIt->features[0] &&
1192 thisIt->features[0]->descriptors.SIFT.size() ==
1193 otherIt->features[0]->descriptors.SIFT.size())
1199 thisIt->getPose(pointPDF_j);
1206 double distMahaFlik2;
1214 pointPDF_k.
mean.z() - pointPDF_j.
mean.z();
1221 distMahaFlik2 = dij.multiply_HCHt_scalar(
1225 exp(K_dist * distMahaFlik2);
1230 if (lik_dist > 1e-2)
1242 mPair(thisIt->ID, otherIt->ID);
1246 n = otherIt->features[0]
1247 ->descriptors.SIFT.size();
1249 for (i = 0; i <
n; i++)
1251 otherIt->features[0]
1252 ->descriptors.SIFT[i] -
1254 ->descriptors.SIFT[i]);
1272 lik_desc = exp(K_desc * desc);
1283 lik = lik_dist * lik_desc;
1313 if (!thisLandmarkAssigned[maxIdx])
1315 thisLandmarkAssigned[maxIdx] =
true;
1318 otherCorrespondences[k] =
true;
1333 correspondences.push_back(match);
1342 correspondencesRatio =
1343 correspondences.size() /
static_cast<float>(nOther);
1364 ->descriptors.SIFT.size();
1366 otherIt != anotherMap->
landmarks.
end(); otherIt++, k++)
1369 unsigned int mEDDidx = 0;
1375 for (i = 0; i < dLen; i++)
1377 otherIt->features[0]->descriptors.SIFT[i] -
1378 thisIt->features[0]->descriptors.SIFT[i]);
1392 if (!thisLandmarkAssigned[mEDDidx])
1396 thisLandmarkAssigned[mEDDidx] =
true;
1399 otherCorrespondences[k] =
true;
1401 otherIt->getPose(pointPDF_k);
1402 thisIt->getPose(pointPDF_j);
1417 correspondences.push_back(match);
1425 correspondencesRatio =
1426 correspondences.size() /
static_cast<float>(nOther);
1442 FILE* f =
os::fopen(file.c_str(),
"wt");
1443 if (!f)
return false;
1456 f,
"%10f %10f %10f %4i %4u %10f", it->pose_mean.x, it->pose_mean.y,
1457 it->pose_mean.z, static_cast<int>(it->getType()),
1461 it->timestampLastSeen));
1465 ASSERT_(!it->features.empty() && it->features[0])
1467 for (
unsigned int i = 0;
1468 i < it->features[0]->descriptors.SIFT.size(); i++)
1469 os::fprintf(f,
" %u ", it->features[0]->descriptors.SIFT[i]);
1487 std::string file,
const char* style,
float confInterval)
const 1489 FILE* f =
os::fopen(file.c_str(),
"wt");
1490 if (!f)
return false;
1494 f,
"%%-------------------------------------------------------\n");
1495 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1496 os::fprintf(f,
"%% 'CLandmarksMap::saveToMATLABScript3D'\n");
1500 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1503 f,
"%%-------------------------------------------------------\n\n");
1512 f,
"m=[%.4f %.4f %.4f];", it->pose_mean.x, it->pose_mean.y,
1515 f,
"c=[%.8f %.8f %.8f;%.8f %.8f %.8f;%.8f %.8f %.8f]; ",
1516 it->pose_cov_11, it->pose_cov_12, it->pose_cov_13, it->pose_cov_12,
1517 it->pose_cov_22, it->pose_cov_23, it->pose_cov_13, it->pose_cov_23,
1522 "error_ellipse(c,m,'conf',%f,'style','%s','numPointsEllipse',10);" 1524 confInterval, style);
1527 os::fprintf(f,
"axis equal;grid on;xlabel('x'); ylabel('y'); zlabel('z');");
1538 std::string file,
const char* style,
float stdCount)
1540 FILE* f =
os::fopen(file.c_str(),
"wt");
1541 if (!f)
return false;
1543 const int ELLIPSE_POINTS = 30;
1544 std::vector<float> X, Y, COS, SIN;
1549 X.resize(ELLIPSE_POINTS);
1550 Y.resize(ELLIPSE_POINTS);
1551 COS.resize(ELLIPSE_POINTS);
1552 SIN.resize(ELLIPSE_POINTS);
1555 for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
1556 Cos++, Sin++, ang += (
M_2PI / (ELLIPSE_POINTS - 1)))
1564 f,
"%%-------------------------------------------------------\n");
1565 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1566 os::fprintf(f,
"%% 'CLandmarksMap::saveToMATLABScript2D'\n");
1570 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1573 f,
"%%-------------------------------------------------------\n\n");
1582 cov(0, 0) = it->pose_cov_11;
1583 cov(1, 1) = it->pose_cov_22;
1584 cov(0, 1) =
cov(1, 0) = it->pose_cov_12;
1586 cov.eigenVectors(eigVec, eigVal);
1587 eigVal = eigVal.array().sqrt().matrix();
1588 M = eigVal * eigVec.transpose();
1592 for (
x = X.begin(),
y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin();
1593 x != X.end();
x++,
y++, Cos++, Sin++)
1597 stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
1600 stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
1606 for (
x = X.begin();
x != X.end();
x++)
1612 for (
y = Y.begin();
y != Y.end();
y++)
1633 unsigned int downSampleFactor)
1637 double J11, J12, J21, J22;
1648 sensorGlobalPose = *robotPose + obs.
sensorPose;
1663 double var_d =
square(0.005);
1664 double var_th =
square(dTh / 10.0);
1667 for (i = 0; i <
n; i += downSampleFactor, Th += downSampleFactor * dTh)
1686 newLandmark.
features[0]->orientation = Th;
1687 newLandmark.
features[0]->scale = d;
1705 newLandmark.
pose_cov_11 = (J11 * J11 * var_th + J12 * J12 * var_d);
1706 newLandmark.
pose_cov_12 = (J11 * J21 * var_th + J12 * J22 * var_d);
1707 newLandmark.
pose_cov_22 = (J21 * J21 * var_th + J22 * J22 * var_d);
1745 double nFoundCorrs = 0;
1747 unsigned int cx, cy, cx_1, cx_2, cy_1, cy_2;
1756 for (itOther =
s->landmarks.begin(); itOther !=
s->landmarks.end();
1759 itOther->getPose(poseOther);
1761 cx = cy = grid->
y2idx(itOther->pose_mean.y);
1763 cx_1 = max(0, grid->
x2idx(itOther->pose_mean.x - 0.10f));
1766 grid->
x2idx(itOther->pose_mean.x + 0.10f));
1767 cy_1 = max(0, grid->
y2idx(itOther->pose_mean.y - 0.10f));
1770 grid->
y2idx(itOther->pose_mean.y + 0.10f));
1781 for (cx = cx_1; cx <= cx_2; cx++)
1782 for (cy = cy_1; cy <= cy_2; cy++)
1786 if (!corrs->empty())
1788 it != corrs->end(); ++it)
1796 if (fabs(lm->
pose_mean.
x - itOther->pose_mean.x) >
1798 fabs(lm->
pose_mean.
y - itOther->pose_mean.y) >
1815 PrNoCorr *= 1 - corr;
1822 nFoundCorrs += 1 - PrNoCorr;
1853 lik *= 1 - PrNoCorr;
1857 lik = nFoundCorrs /
static_cast<double>(
s->size());
1874 m_grid(-10.0f, 10.0f, -10.0f, 10.f, 0.20f),
1875 m_largestDistanceFromOrigin(),
1876 m_largestDistanceFromOriginIsUpdated(false)
1882 m_landmarks.clear();
1887 m_largestDistanceFromOriginIsUpdated =
false;
1899 max(m_grid.getYMax(), l.
pose_mean.
y + 0.1), dummyEmpty);
1901 m_landmarks.push_back(l);
1906 cell->push_back(m_landmarks.size() - 1);
1908 m_largestDistanceFromOriginIsUpdated =
false;
1913 return &m_landmarks[indx];
1917 unsigned int indx)
const 1919 return &m_landmarks[indx];
1925 m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1928 for (it = cell->begin(); it != cell->end(); it++)
1930 if (*it == static_cast<int>(indx))
1937 m_largestDistanceFromOriginIsUpdated =
false;
1942 m_landmarks.erase(m_landmarks.begin() + indx);
1943 m_largestDistanceFromOriginIsUpdated =
false;
1952 min(m_grid.getXMin(), m_landmarks[indx].pose_mean.x),
1953 max(m_grid.getXMax(), m_landmarks[indx].pose_mean.x),
1954 min(m_grid.getYMin(), m_landmarks[indx].pose_mean.y),
1955 max(m_grid.getYMax(), m_landmarks[indx].pose_mean.y), dummyEmpty);
1959 m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1960 cell->push_back(indx);
1961 m_largestDistanceFromOriginIsUpdated =
false;
1970 double min_x = -10.0, max_x = 10.0;
1971 double min_y = -10.0, max_y = 10.0;
1978 for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1981 min_x =
min(min_x, it->pose_mean.x);
1982 max_x = max(max_x, it->pose_mean.x);
1983 min_y =
min(min_y, it->pose_mean.y);
1984 max_y = max(max_y, it->pose_mean.y);
1986 m_grid.resize(min_x, max_x, min_y, max_y, dummyEmpty);
1989 for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1992 vector_int* cell = m_grid.cellByPos(it->pose_mean.x, it->pose_mean.y);
1993 cell->push_back(idx);
1996 m_largestDistanceFromOriginIsUpdated =
false;
2007 if (!m_largestDistanceFromOriginIsUpdated)
2010 float maxDistSq = 0, d;
2013 d =
square(it->pose_mean.x) +
square(it->pose_mean.y) +
2015 maxDistSq = max(d, maxDistSq);
2018 m_largestDistanceFromOrigin = sqrt(maxDistSq);
2019 m_largestDistanceFromOriginIsUpdated =
true;
2021 return m_largestDistanceFromOrigin;
2029 std::vector<bool>* otherCorrespondences)
2039 unsigned long distDesc;
2040 double likByDist, likByDesc;
2060 unsigned int idx1, idx2;
2062 CMatrixD dij(1, 3), Cij(3, 3), Cij_1;
2063 double distMahaFlik2;
2076 lm1 += decimation, idx1 += decimation)
2081 lm1->getPose(lm1_pose);
2092 lm2->getPose(lm2_pose);
2097 dij(0, 0) = lm1->pose_mean.x - lm2->pose_mean.x;
2098 dij(0, 1) = lm1->pose_mean.y - lm2->pose_mean.y;
2099 dij(0, 2) = lm1->pose_mean.z - lm2->pose_mean.z;
2108 distMahaFlik2 = dij.multiply_HCHt_scalar(
2111 likByDist = exp(K_dist * distMahaFlik2);
2114 if (likByDist > 1e-2)
2125 mPair(lm2->ID, lm1->ID);
2133 !lm1->features.empty() &&
2134 !lm2->features.empty())
2136 lm1->features[0] && lm2->features[0])
2139 ->descriptors.SIFT.size() ==
2141 ->descriptors.SIFT.size())
2143 for (it1 = lm1->features[0]
2144 ->descriptors.SIFT.begin(),
2145 it2 = lm2->features[0]
2146 ->descriptors.SIFT.begin();
2149 ->descriptors.SIFT.end();
2151 distDesc +=
square(*it1 - *it2);
2164 distDesc = (
unsigned long)
2171 likByDesc = exp(K_desc * distDesc);
2189 lik_i += likByDist *
2207 lik *= (0.1 + 0.9 * lik_i);
2233 if (correspondences ==
nullptr)
2235 "When using this method with SIFTLikelihoodMethod = 1, " 2236 "TMatchingPairList *correspondence can not be NULL");
2238 if (otherCorrespondences ==
nullptr)
2240 "When using this method with SIFTLikelihoodMethod = 1, " 2241 "std::vector<bool> *otherCorrespondences can not be NULL");
2243 for (itCorr = correspondences->begin();
2244 itCorr != correspondences->end(); itCorr++)
2257 distMahaFlik2 = dij.multiply_HCHt_scalar(
2264 lik *= exp(-0.5 * dist);
2269 for (k = 1; k <= (theMap->
size() - correspondences->size()); k++)
2292 : insert_SIFTs_from_monocular_images(true),
2293 insert_SIFTs_from_stereo_images(true),
2294 insert_Landmarks_from_range_scans(true),
2295 SiftCorrRatioThreshold(0.4f),
2296 SiftLikelihoodThreshold(0.5f),
2298 SiftEDDThreshold(200.0f),
2299 SIFTMatching3DMethod(0),
2300 SIFTLikelihoodMethod(0),
2302 SIFTsLoadDistanceOfTheMean(3.0f),
2303 SIFTsLoadEllipsoidWidth(0.05f),
2306 SIFTs_stdDisparity(1.0f),
2308 SIFTs_numberOfKLTKeypoints(60),
2309 SIFTs_stereo_maxDepth(15.0f),
2311 SIFTs_epipolar_TH(1.5f),
2314 SIFT_feat_options(vision::
featSIFT)
2325 "\n----------- [CLandmarksMap::TInsertionOptions] ------------ \n\n");
2328 "insert_SIFTs_from_monocular_images = %c\n",
2329 insert_SIFTs_from_monocular_images ?
'Y' :
'N');
2331 "insert_SIFTs_from_stereo_images = %c\n",
2332 insert_SIFTs_from_stereo_images ?
'Y' :
'N');
2334 "insert_Landmarks_from_range_scans = %c\n",
2335 insert_Landmarks_from_range_scans ?
'Y' :
'N');
2338 "SiftCorrRatioThreshold = %f\n",
2339 SiftCorrRatioThreshold);
2341 "SiftLikelihoodThreshold = %f\n",
2342 SiftLikelihoodThreshold);
2344 "SiftEDDThreshold = %f\n", SiftEDDThreshold);
2346 "SIFTMatching3DMethod = %d\n", SIFTMatching3DMethod);
2348 "SIFTLikelihoodMethod = %d\n", SIFTLikelihoodMethod);
2351 "SIFTsLoadDistanceOfTheMean = %f\n",
2352 SIFTsLoadDistanceOfTheMean);
2354 "SIFTsLoadEllipsoidWidth = %f\n",
2355 SIFTsLoadEllipsoidWidth);
2357 out.
printf(
"SIFTs_stdXY = %f\n", SIFTs_stdXY);
2359 "SIFTs_stdDisparity = %f\n", SIFTs_stdDisparity);
2362 "SIFTs_numberOfKLTKeypoints = %i\n",
2363 SIFTs_numberOfKLTKeypoints);
2365 "SIFTs_stereo_maxDepth = %f\n",
2366 SIFTs_stereo_maxDepth);
2368 "SIFTs_epipolar_TH = %f\n", SIFTs_epipolar_TH);
2370 "PLOT_IMAGES = %c\n",
2371 PLOT_IMAGES ?
'Y' :
'N');
2373 SIFT_feat_options.dumpToTextStream(out);
2384 insert_SIFTs_from_monocular_images = iniFile.
read_bool(
2385 section.c_str(),
"insert_SIFTs_from_monocular_images",
2386 insert_SIFTs_from_monocular_images);
2387 insert_SIFTs_from_stereo_images = iniFile.
read_bool(
2388 section.c_str(),
"insert_SIFTs_from_stereo_images",
2389 insert_SIFTs_from_stereo_images);
2390 insert_Landmarks_from_range_scans = iniFile.
read_bool(
2391 section.c_str(),
"insert_Landmarks_from_range_scans",
2392 insert_Landmarks_from_range_scans);
2394 section.c_str(),
"SiftCorrRatioThreshold", SiftCorrRatioThreshold);
2395 SiftLikelihoodThreshold = iniFile.
read_float(
2396 section.c_str(),
"SiftLikelihoodThreshold", SiftLikelihoodThreshold);
2398 section.c_str(),
"SiftEDDThreshold", SiftEDDThreshold);
2399 SIFTMatching3DMethod = iniFile.
read_int(
2400 section.c_str(),
"SIFTMatching3DMethod", SIFTMatching3DMethod);
2401 SIFTLikelihoodMethod = iniFile.
read_int(
2402 section.c_str(),
"SIFTLikelihoodMethod", SIFTLikelihoodMethod);
2403 SIFTsLoadDistanceOfTheMean = iniFile.
read_float(
2404 section.c_str(),
"SIFTsLoadDistanceOfTheMean",
2405 SIFTsLoadDistanceOfTheMean);
2406 SIFTsLoadEllipsoidWidth = iniFile.
read_float(
2407 section.c_str(),
"SIFTsLoadEllipsoidWidth", SIFTsLoadEllipsoidWidth);
2409 iniFile.
read_float(section.c_str(),
"SIFTs_stdXY", SIFTs_stdXY);
2411 section.c_str(),
"SIFTs_stdDisparity", SIFTs_stdDisparity);
2412 SIFTs_numberOfKLTKeypoints = iniFile.
read_int(
2413 section.c_str(),
"SIFTs_numberOfKLTKeypoints",
2414 SIFTs_numberOfKLTKeypoints);
2416 section.c_str(),
"SIFTs_stereo_maxDepth", SIFTs_stereo_maxDepth);
2418 section.c_str(),
"SIFTs_epipolar_TH", SIFTs_epipolar_TH);
2420 iniFile.
read_bool(section.c_str(),
"PLOT_IMAGES", PLOT_IMAGES);
2422 SIFT_feat_options.loadFromConfigFile(iniFile, section);
2429 : rangeScan2D_decimation(20),
2430 SIFTs_sigma_euclidean_dist(0.30f),
2431 SIFTs_sigma_descriptor_dist(100.0f),
2432 SIFTs_mahaDist_std(4.0f),
2433 SIFTnullCorrespondenceDistance(4.0f),
2434 SIFTs_decimation(1),
2435 SIFT_feat_options(vision::
featSIFT),
2436 beaconRangesStd(0.08f),
2437 beaconRangesUseObservationStd(false),
2438 extRobotPoseStd(0.05f),
2445 : longitude(-4.47763833333333),
2446 latitude(36.71559000000000),
2462 "\n----------- [CLandmarksMap::TLikelihoodOptions] ------------ \n\n");
2465 "rangeScan2D_decimation = %i\n",
2468 "SIFTs_sigma_euclidean_dist = %f\n",
2471 "SIFTs_sigma_descriptor_dist = %f\n",
2478 "SIFTnullCorrespondenceDistance = %f\n",
2483 "beaconRangesUseObservationStd = %c\n",
2518 section.c_str(),
"SIFTs_sigma_euclidean_dist",
2521 section.c_str(),
"SIFTs_sigma_descriptor_dist",
2528 section.c_str(),
"SIFTnullCorrespondenceDistance",
2552 section.c_str(),
"beaconRangesUseObservationStd",
2565 : minTimesSeen(2), ellapsedTime(4.0f)
2577 const CPose3D& in_robotPose,
const CPoint3D& in_sensorLocationOnRobot,
2586 point3D = in_robotPose + in_sensorLocationOnRobot;
2598 it->getPose(beaconPDF);
2599 beacon3D = beaconPDF.
mean;
2617 out_Observations.
sensedData.push_back(newMeas);
2639 mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
2643 mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
2644 -100, 100, -100, 100, 0, 1);
2672 mrpt::make_aligned_shared<opengl::CEllipsoid>();
2674 it->getPose(pointGauss);
2676 ellip->setPose(pointGauss.
mean);
2677 ellip->setCovMatrix(pointGauss.
cov);
2678 ellip->enableDrawSolid3D(
false);
2679 ellip->setQuantiles(3.0);
2680 ellip->set3DsegmentsCount(10);
2681 ellip->setColor(0, 0, 1);
2683 mrpt::format(
"LM.ID=%u", static_cast<unsigned int>(it->ID)));
2684 ellip->enableShowName(
true);
2686 outObj->insert(ellip);
2699 for (
size_t indx = 0; indx < m_landmarks.size(); indx++)
2701 if (m_landmarks[indx].ID == ID)
return &m_landmarks[indx];
2718 unsigned int ID)
const 2720 for (
size_t indx = 0; indx < m_landmarks.size(); indx++)
2722 if (m_landmarks[indx].ID == ID)
return &m_landmarks[indx];
2755 otherMap = static_cast<const CLandmarksMap*>(otherMap2);
2757 if (!otherMap)
return 0;
2760 std::deque<CPointPDFGaussian::Ptr> poses3DThis, poses3DOther;
2764 size_t nOther = otherMap->landmarks.size();
2765 size_t otherLandmarkWithCorrespondence = 0;
2768 if (!nThis)
return 0;
2769 if (!nOther)
return 0;
2774 float Tx = pose3DMatrix.get_unsafe(0, 3);
2775 float Ty = pose3DMatrix.get_unsafe(1, 3);
2776 float Tz = pose3DMatrix.get_unsafe(2, 3);
2786 otherMap->landmarks.getLargestDistanceFromOrigin() + 1.0f))
2790 poses3DOther.resize(nOther);
2791 for (
size_t i = 0; i < nOther; i++)
2792 poses3DOther[i] = mrpt::make_aligned_shared<CPointPDFGaussian>();
2794 poses3DThis.resize(nThis);
2795 for (
size_t i = 0; i < nThis; i++)
2796 poses3DThis[i] = mrpt::make_aligned_shared<CPointPDFGaussian>();
2799 for (itOther = otherMap->landmarks.begin(),
2800 itPoseOther = poses3DOther.begin();
2801 itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2803 itOther->getPose(**itPoseOther);
2804 (*itPoseOther)->changeCoordinatesReference(otherMapPose);
2808 for (itThis =
landmarks.
begin(), itPoseThis = poses3DThis.begin();
2811 itThis->getPose(**itPoseThis);
2815 for (itOther = otherMap->landmarks.begin(),
2816 itPoseOther = poses3DOther.begin();
2817 itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2819 for (itThis =
landmarks.
begin(), itPoseThis = poses3DThis.begin();
2825 (*itPoseThis)->mean.x() - (*itPoseOther)->mean.x(),
2826 (*itPoseThis)->mean.y() - (*itPoseOther)->mean.y(),
2827 (*itPoseThis)->mean.z() - (*itPoseOther)->mean.z());
2833 float distMaha = sqrt(
2834 d.multiply_HCHt_scalar(
2837 if (distMaha <
params.maxMahaDistForCorr)
2840 if (!itThis->features.empty() && !itOther->features.empty() &&
2841 itThis->features[0]->descriptors.SIFT.size() ==
2842 itOther->features[0]->descriptors.SIFT.size())
2844 unsigned long descrDist = 0;
2846 for (it1 = itThis->features[0]->descriptors.SIFT.begin(),
2847 it2 = itOther->features[0]->descriptors.SIFT.begin();
2848 it1 != itThis->features[0]->descriptors.SIFT.end();
2850 descrDist +=
square(*it1 - *it2);
2853 sqrt(static_cast<float>(descrDist)) /
2854 itThis->features[0]->descriptors.SIFT.size();
2856 if (descrDist_f < 1.5f)
2858 otherLandmarkWithCorrespondence++;
2867 return static_cast<float>(otherLandmarkWithCorrespondence) / nOther;
2895 const CPose3D& in_robotPose,
const CPose3D& in_sensorLocationOnRobot,
2897 const float in_stdRange,
const float in_stdYaw,
const float in_stdPitch,
2898 vector_size_t* out_real_associations,
const double spurious_count_mean,
2899 const double spurious_count_std)
const 2907 if (out_real_associations) out_real_associations->clear();
2910 const CPose3D point3D = in_robotPose +
CPose3D(in_sensorLocationOnRobot);
2927 it->getPose(beaconPDF);
2948 if (sensorDetectsIDs)
2957 out_Observations.
sensedData.push_back(newMeas);
2959 if (out_real_associations)
2960 out_real_associations->push_back(idx);
2964 const double fSpurious =
2966 size_t nSpurious = 0;
2967 if (spurious_count_std != 0 || spurious_count_mean != 0)
2968 nSpurious =
static_cast<size_t>(
2973 for (
size_t i = 0; i < nSpurious; i++)
2986 const double pitch =
3001 out_Observations.
sensedData.push_back(newMeas);
3003 if (out_real_associations)
3004 out_real_associations->push_back(
void clear()
Erase all the contents of the map.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
A namespace of pseudo-random numbers genrators of diferent distributions.
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::utils::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
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.
A pair (x,y) of pixel coordinates (subpixel resolution).
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
double x() const
Common members of all points & poses classes.
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
TFuseOptions()
Initialization.
mrpt::utils::CDynamicGrid< vector_int > * getGrid()
Parameters for CMetricMap::compute3DMatchingRatio()
float SiftEDDThreshold
[For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as corres...
Non-defined feature (also used for Occupancy features)
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
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void computeMatchingWith3DLandmarks(const mrpt::maps::CLandmarksMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: In this class...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose3D mean
The mean value.
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
void createOneFeature()
Creates one feature in the vector "features", calling the appropriate constructor of the smart pointe...
mrpt::utils::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
std::vector< mrpt::vision::CFeature::Ptr > features
The set of features from which the landmark comes.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
float range
The sensed landmark distance, in meters.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
static bool _maxIDUpdated
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
float beaconRangesStd
The standard deviation used for Beacon ranges likelihood (default=0.08) [meters]. ...
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::utils::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
A class for storing images as grayscale or RGB bitmaps.
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPoint3D mean
The mean value.
float SIFTs_stereo_maxDepth
Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
#define MRPT_CHECK_NORMAL_NUMBER(val)
void fuseWith(CLandmarksMap &other, bool justInsertAllOfThem=false)
Fuses the contents of another map with this one, updating "this" object with the result.
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
int x2idx(double x) const
Transform a coordinate values into cell indexes.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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.
struct mrpt::maps::CLandmarksMap::TFuseOptions fuseOptions
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
double norm() const
Point norm.
EIGEN_STRONG_INLINE iterator begin()
long round_long(const T value)
Returns the closer integer (long) to x.
float extRobotPoseStd
The standard deviation used for external robot poses likelihood (default=0.05) [meters].
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)
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
TMapGenericParams genericMapParams
Common params to all maps.
internal::TSequenceLandmarks::iterator iterator
float epipolar_TH
Epipolar constraint (rows of pixels)
const Scalar * const_iterator
float minZ
Maximum allowed distance.
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
int64_t TLandmarkID
The type for the IDs of landmarks.
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.
mrpt::poses::CPose3D sensorPose
The pose of the sensor on the robot/vehicle.
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.
void internal_clear() override
Internal method called by clear()
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
int y2idx(double y) const
bool beaconRangesUseObservationStd
(Default: false) If true, beaconRangesStd is ignored and each individual CObservationBeaconRanges::st...
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
float stdPixel
Standard deviation of the error in feature detection.
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
bool insert_SIFTs_from_monocular_images
If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D feature...
mrpt::math::TPoint3D pixelTo3D(const mrpt::utils::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...
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
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).
float SIFTsLoadDistanceOfTheMean
[For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m)
Matching by Euclidean distance between SIFT descriptors.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
int SIFTs_numberOfKLTKeypoints
Number of points to extract in the image.
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)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
double norm() const
Returns the euclidean norm of vector: .
This CStream derived class allow using a file as a write-only, binary stream.
Parameters associated to a stereo system.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
float SIFTs_stdXY
[For SIFT landmarks only] The standard deviation (in pixels) for the SIFTs detector (This is used for...
Scale Invariant Feature Transform [LOWE'04].
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
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 )...
std::shared_ptr< CSetOfObjects > Ptr
virtual ~CLandmarksMap()
Virtual destructor.
This namespace contains representation of robot actions and observations.
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Classes for computer vision, detectors, features, etc.
double x
X,Y,Z coordinates.
void getPose(mrpt::poses::CPointPDFGaussian &p) const
Returns the pose as an object:
float SIFTnullCorrespondenceDistance
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the wor...
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
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...
MSG_CLASS & getMsgByClass()
Returns a reference to the message in the list CObservationGPS::messages of the requested class...
A class used to store a 3D point.
mrpt::utils::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
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::utils::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
mrpt::system::TTimeStamp timestampLastSeen
The last time that this landmark was observed.
mrpt::poses::CPose3D sensorLocationOnRobot
The position of the sensor on the robot.
Each one of the measurements:
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
unsigned int minTimesSeen
Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds...
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.
bool saveToTextFile(std::string file)
Save to a text file.
void hasBeenModified(unsigned int indx)
float sensedDistance
The sensed range itself (in meters).
unsigned int rangeScan2D_decimation
The number of rays from a 2D range scan will be decimated by this factor (default = 1...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
mrpt::poses::CPose3D refCameraPose
The 3D pose of the reference camera relative to robot coordinates.
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.
CMatrixTemplateNumeric< T > generateAxisBaseFromDirection(T dx, T dy, T dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::poses::CPose3DPDFGaussian pose
The observed robot pose.
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
float ellapsedTime
See "minTimesSeen".
internal_msg_test_proxy< gnss::NMEA_GGA > has_GGA_datum
Evaluates as a bool; true if the corresponding field exists in messages.
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...
bool PLOT_IMAGES
Indicates if the images (as well as the SIFT detected features) should be shown in a window...
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.
float SIFTsLoadEllipsoidWidth
[For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perp...
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...
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< 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...
float maxZ
Maximum allowed distance.
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::utils::DEG2RAD(0.1f), const float stdPitch=mrpt::utils::DEG2RAD(0.1f), 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 ...
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
mrpt::vision::TFeatureType getType() const
Gets the type of the first feature in its feature vector.
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.
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
A structure containing options for the matching.
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
TInsertionOptions()
Initilization of default parameters.
mrpt::maps::CLandmarksMap landmarks
The landmarks, with coordinates origin in the camera reference system.
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)...
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
mrpt::utils::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
#define INVALID_LANDMARK_ID
Used for CObservationBearingRange::TMeasurement::beaconID and others.
virtual void auxParticleFilterCleanUp() override
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
TInternalFeatList::iterator iterator
std::vector< size_t > vector_size_t
Each one of the measurements.
void computeMatchingWith2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const mrpt::poses::CPose2D &angularDistPivotPoint, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=nullptr, bool onlyKeepTheClosest=false, bool onlyUniqueRobust=false) const
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
internal::TSequenceLandmarks::const_iterator const_iterator
double computeLikelihood_RSLC_2007(const CLandmarksMap *s, const mrpt::poses::CPose2D &sensorPose)
Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
std::vector< int32_t > vector_int
CLandmark * get(unsigned int indx)
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
bool saveToMATLABScript2D(std::string file, const char *style="b", float stdCount=2.0f)
Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY ...
A especial case: this is not an image feature, but a 2D/3D beacon (used for range-only SLAM from mrpt...
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
TCustomSequenceLandmarks()
Default constructor.
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.
struct mrpt::maps::CLandmarksMap::TLikelihoodOptions::TGPSOrigin GPSOrigin
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters.
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
float SIFTs_epipolar_TH
Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential mat...
TMatchingMethod matching_method
Matching method.
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
float SiftLikelihoodThreshold
[For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0...
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
unsigned __int32 uint32_t
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
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)...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
double ang
These 3 params allow rotating and shifting GPS coordinates with other 2D maps (e.g.
GLenum const GLfloat * params
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
float SiftCorrRatioThreshold
[For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as c...
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.
double SIFTs_sigma_euclidean_dist
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
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.
std::shared_ptr< CGridPlaneXY > Ptr
unsigned int min_sat
Minimum number of sats to take into account the data.
A gaussian distribution for 3D points.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
float GPS_sigma
A constant "sigma" for GPS localization data (in meters)
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
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:
std::shared_ptr< CEllipsoid > Ptr
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
std::vector< CLandmark > TSequenceLandmarks
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOptions
double computeLikelihood_SIFT_LandmarkMap(CLandmarksMap *map, mrpt::utils::TMatchingPairList *correspondences=nullptr, std::vector< bool > *otherCorrespondences=nullptr)
Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
unsigned int SIFTMatching3DMethod
[For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method)) 0: Our method -> E...