29 assert(hist1.size() == hist2.size());
31 double BhattachDist_aux = 0.0;
32 for (
unsigned i = 0; i < hist1.size(); i++)
33 BhattachDist_aux += sqrt(hist1[i] * hist2[i]);
35 BhattachDist = sqrt(1 - BhattachDist_aux);
40 SubgraphMatcher::SubgraphMatcher()
48 bool SubgraphMatcher::evalUnaryConstraints(
56 cout <<
"unary between " << plane1.
id <<
" " << plane2.
id <<
"\n";
101 configLocaliser.hue_threshold)
104 cout <<
"Hist_H false "
106 << configLocaliser.hue_threshold <<
"\n";
135 if (rel_areas < configLocaliser.area_full_threshold_inv ||
136 rel_areas > configLocaliser.area_full_threshold)
139 cout <<
"False: rel_areas full " << rel_areas << endl;
146 if (rel_elong < configLocaliser.elongation_threshold_inv ||
147 rel_elong > configLocaliser.elongation_threshold)
150 cout <<
"False: rel_elong full " << rel_elong << endl;
160 if (rel_areas < configLocaliser.area_full_threshold_inv ||
161 rel_areas > configLocaliser.area_threshold)
164 cout <<
"rel_areas RefFull " << rel_areas << endl;
171 if (rel_areas < configLocaliser.area_threshold_inv ||
172 rel_areas > configLocaliser.area_full_threshold)
175 cout <<
"rel_areas CheckFull " << rel_areas << endl;
181 rel_areas < configLocaliser.area_threshold_inv ||
182 rel_areas > configLocaliser.area_threshold)
185 cout <<
"rel_areas simple " << rel_areas << endl;
191 if (rel_elong < configLocaliser.elongation_threshold_inv ||
192 rel_elong > configLocaliser.elongation_threshold)
195 cout <<
"rel_elong simple " << rel_elong << endl;
204 cout <<
"UNARY TRUE" << endl;
210 bool SubgraphMatcher::evalUnaryConstraintsOdometry(
218 cout <<
"unaryOdometry between " << plane1.
id <<
" " << plane2.
id <<
"\n";
224 if (fabs(plane1.
d - plane2.
d) > configLocaliser.dist_d)
227 cout <<
"depthUnaryConstraint false " << fabs(plane1.
d - plane2.
d)
228 <<
" > " << configLocaliser.dist_d <<
"\n";
236 cout <<
"angleUnaryConstraint false "
238 << configLocaliser.angle <<
"\n";
284 configLocaliser.hue_threshold)
287 cout <<
"Hist_H false_ "
289 << configLocaliser.hue_threshold <<
"\n";
321 if (rel_areas < configLocaliser.area_full_threshold_inv ||
322 rel_areas > configLocaliser.area_full_threshold)
325 cout <<
"False: rel_areas full " << rel_areas << endl;
332 if (rel_elong < configLocaliser.elongation_threshold_inv ||
333 rel_elong > configLocaliser.elongation_threshold)
336 cout <<
"False: rel_elong full " << rel_elong << endl;
346 if (rel_areas < configLocaliser.area_full_threshold_inv ||
347 rel_areas > configLocaliser.area_threshold)
350 cout <<
"rel_areas RefFull " << rel_areas << endl;
357 if (rel_areas < configLocaliser.area_threshold_inv ||
358 rel_areas > configLocaliser.area_full_threshold)
361 cout <<
"rel_areas CheckFull " << rel_areas << endl;
367 rel_areas < configLocaliser.area_threshold_inv ||
368 rel_areas > configLocaliser.area_threshold)
371 cout <<
"rel_areas simple " << rel_areas << endl;
377 if (rel_elong < configLocaliser.elongation_threshold_inv ||
378 rel_elong > configLocaliser.elongation_threshold)
381 cout <<
"rel_elong simple " << rel_elong << endl;
390 cout <<
"UNARY TRUE" << endl;
396 bool SubgraphMatcher::evalUnaryConstraints2D(
404 cout <<
"unary2D between " << plane1.
id <<
" " << plane2.
id <<
"\n";
414 if (fabs(plane1.
d - plane2.
d) < 0.15)
463 configLocaliser.hue_threshold)
466 cout <<
"Hist_H false "
468 << configLocaliser.hue_threshold <<
"\n";
497 if (rel_areas < configLocaliser.area_full_threshold_inv ||
498 rel_areas > configLocaliser.area_full_threshold)
501 cout <<
"False: rel_areas full " << rel_areas << endl;
508 if (rel_elong < configLocaliser.elongation_threshold_inv ||
509 rel_elong > configLocaliser.elongation_threshold)
512 cout <<
"False: rel_elong full " << rel_elong << endl;
522 if (rel_areas < configLocaliser.area_full_threshold_inv ||
523 rel_areas > configLocaliser.area_threshold)
526 cout <<
"rel_areas RefFull " << rel_areas << endl;
533 if (rel_areas < configLocaliser.area_threshold_inv ||
534 rel_areas > configLocaliser.area_full_threshold)
537 cout <<
"rel_areas CheckFull " << rel_areas << endl;
543 rel_areas < configLocaliser.area_threshold_inv ||
544 rel_areas > configLocaliser.area_threshold)
547 cout <<
"rel_areas simple " << rel_areas << endl;
553 if (rel_elong < configLocaliser.elongation_threshold_inv ||
554 rel_elong > configLocaliser.elongation_threshold)
557 cout <<
"rel_elong simple " << rel_elong << endl;
566 cout <<
"UNARY TRUE" << endl;
572 bool SubgraphMatcher::evalUnaryConstraintsOdometry2D(
580 cout <<
"unaryOdometry2D between " << plane1.
id <<
" " << plane2.
id <<
"\n";
591 if (fabs(plane1.
d - plane2.
d) < 0.12)
598 cout <<
"pass 2D \n";
602 if (fabs(plane1.
d - plane2.
d) > configLocaliser.dist_d)
605 cout <<
"depthUnaryConstraint false " << fabs(plane1.
d - plane2.
d)
606 <<
" > " << configLocaliser.dist_d <<
"\n";
614 cout <<
"angleUnaryConstraint false "
616 << configLocaliser.angle <<
"\n";
662 configLocaliser.hue_threshold)
665 cout <<
"Hist_H false_ "
667 << configLocaliser.hue_threshold <<
"\n";
699 if (rel_areas < configLocaliser.area_full_threshold_inv ||
700 rel_areas > configLocaliser.area_full_threshold)
703 cout <<
"False: rel_areas full " << rel_areas << endl;
710 if (rel_elong < configLocaliser.elongation_threshold_inv ||
711 rel_elong > configLocaliser.elongation_threshold)
714 cout <<
"False: rel_elong full " << rel_elong << endl;
724 if (rel_areas < configLocaliser.area_full_threshold_inv ||
725 rel_areas > configLocaliser.area_threshold)
728 cout <<
"rel_areas RefFull " << rel_areas << endl;
735 if (rel_areas < configLocaliser.area_threshold_inv ||
736 rel_areas > configLocaliser.area_full_threshold)
739 cout <<
"rel_areas CheckFull " << rel_areas << endl;
745 rel_areas < configLocaliser.area_threshold_inv ||
746 rel_areas > configLocaliser.area_threshold)
749 cout <<
"rel_areas simple " << rel_areas << endl;
755 if (rel_elong < configLocaliser.elongation_threshold_inv ||
756 rel_elong > configLocaliser.elongation_threshold)
759 cout <<
"rel_elong simple " << rel_elong << endl;
768 cout <<
"UNARY TRUE" << endl;
778 bool SubgraphMatcher::evalBinaryConstraints(
793 double dif_height = fabs(
798 float threshold_dist_dependent =
799 configLocaliser.height_threshold *
801 float height_threshold = std::max(
802 configLocaliser.height_threshold, threshold_dist_dependent);
803 if (dif_height > height_threshold)
807 cout <<
"Binary false: Ref dif_height " << dif_height << endl;
815 double dif_height = fabs(
823 configLocaliser.height_threshold;
824 if (dif_height > height_threshold)
828 cout <<
"Binary false BIG Area: Ref dif_height " << dif_height
838 double dif_height2 = fabs(
843 float threshold_dist_dependent =
844 configLocaliser.height_threshold *
846 float height_threshold = std::max(
847 configLocaliser.height_threshold, threshold_dist_dependent);
848 if (dif_height2 > height_threshold)
852 cout <<
"Binary false: neigRef dif_height2 " << dif_height2 << endl;
860 double dif_height2 = fabs(
867 float height_threshold =
869 if (dif_height2 > height_threshold)
873 cout <<
"Binary false: neigRef dif_height2 " << dif_height2 << endl;
881 double dif_normal = fabs(
886 if (dif_normal > std::max(
887 configLocaliser.angle_threshold,
891 cout <<
"Binary false: angle " << dif_normal <<
" with " << neigRef.
id
899 double rel_dist_centers = sqrt(
911 if (configLocaliser.use_completeness)
913 if (RefBothFull && CheckBothFull)
915 if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
916 rel_dist_centers > configLocaliser.dist_threshold)
919 cout <<
"Binary false: dist_centers1 with " << neigRef.
id
926 else if (RefBothFull || CheckBothFull)
928 if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
929 rel_dist_centers > configLocaliser.dist_threshold)
932 cout <<
"Binary false: dist_centers2 with " << neigRef.
id
942 if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
943 rel_dist_centers > configLocaliser.dist_threshold)
946 cout <<
"Binary false: dist_centers3 with " << neigRef.
id
959 if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
960 rel_dist_centers > configLocaliser.dist_threshold)
963 cout <<
"Binary false: dist_centers4 with " << neigRef.
id
975 cout <<
"BINARY TRUE\n" << endl;
982 map<unsigned, unsigned>& contained, map<unsigned, unsigned>& container)
984 if (contained.size() > container.size())
return false;
988 it != contained.end(); it++)
989 if (container.count(it->first) == 0)
991 else if (container[it->first] != it->second)
995 cout <<
"Repeated sequence. Comparing:\n";
997 it != contained.end(); it++)
998 cout << it->first <<
" " << it->second << endl;
1001 it != container.end(); it++)
1002 cout << it->first <<
" " << it->second << endl;
1015 void SubgraphMatcher::exploreSubgraphTreeR(
1016 set<unsigned>& sourcePlanes, set<unsigned>& targetPlanes,
1017 map<unsigned, unsigned>& matched)
1020 cout <<
"matched: " << matched.size() <<
"\n";
1022 it != matched.end(); it++)
1023 cout << it->first <<
" - " << it->second <<
" ="
1024 << subgraphTrg->pPBM->vPlanes[it->second].label << endl;
1025 cout <<
"sourcePlanes: " << sourcePlanes.size() <<
": ";
1027 it1 != sourcePlanes.end(); it1++)
1028 cout << *it1 <<
" ";
1029 cout <<
"\ntargetPlanes " << targetPlanes.size() <<
": ";
1031 it2 != targetPlanes.end(); it2++)
1032 cout << *it2 <<
" ";
1042 unsigned requiredMatches =
1043 max(configLocaliser.min_planes_recognition,
1044 static_cast<unsigned>(winnerMatch.size()));
1065 while (!sourcePlanes.empty())
1069 cout <<
"Compare " << *it1
1070 <<
" Compare Compare Compare Compare Compare " << endl;
1073 if ((matched.size() +
min(sourcePlanes.size(), targetPlanes.size())) <=
1077 cout <<
"End branch recursive search. Too short " << matched.size()
1078 <<
" prev winner " << winnerMatch.size() << endl;
1095 it2 != targetPlanes.end(); it2++)
1098 cout <<
" " << *it1 <<
" with " << *it2 << endl;
1132 if (hashUnaryConstraints[*it1][*it2] != 1)
1137 bool binaryFail =
false;
1139 it_matched != matched.end(); it_matched++)
1140 if (!evalBinaryConstraints(
1141 subgraphSrc->pPBM->vPlanes[*it1],
1142 subgraphSrc->pPBM->vPlanes[it_matched->first],
1143 subgraphTrg->pPBM->vPlanes[*it2],
1144 subgraphTrg->pPBM->vPlanes[it_matched->second]))
1149 if (binaryFail)
continue;
1154 set<unsigned> nextSrcPlanes = sourcePlanes;
1155 nextSrcPlanes.erase(*it1);
1156 set<unsigned> nextTrgPlanes = targetPlanes;
1157 nextTrgPlanes.erase(*it2);
1158 map<unsigned, unsigned> nextMatched = matched;
1159 nextMatched[*it1] = *it2;
1161 alreadyExplored.push_back(nextMatched);
1163 exploreSubgraphTreeR(nextSrcPlanes, nextTrgPlanes, nextMatched);
1169 sourcePlanes.erase(it1);
1172 if (matched.size() > winnerMatch.size())
1174 float areaMatched = calcAreaMatched(matched);
1177 cout <<
"End branch recursive search. matched " << matched.size()
1178 <<
" A " << areaMatched <<
" prev winner " << winnerMatch.size()
1179 <<
" A " << areaWinnerMatch << endl;
1181 areaWinnerMatch = areaMatched;
1182 winnerMatch = matched;
1186 void SubgraphMatcher::exploreSubgraphTreeR_Area(
1187 set<unsigned>& sourcePlanes, set<unsigned>& targetPlanes,
1188 map<unsigned, unsigned>& matched)
1191 cout <<
"matched: " << matched.size() <<
"\n";
1193 it != matched.end(); it++)
1194 cout << it->first <<
" - " << it->second <<
" ="
1195 << subgraphTrg->pPBM->vPlanes[it->second].label << endl;
1196 cout <<
"sourcePlanes: " << sourcePlanes.size() <<
": ";
1198 it1 != sourcePlanes.end(); it1++)
1199 cout << *it1 <<
" ";
1200 cout <<
"\ntargetPlanes " << targetPlanes.size() <<
": ";
1202 it2 != targetPlanes.end(); it2++)
1203 cout << *it2 <<
" ";
1213 float matchedArea = calcAreaMatched(matched);
1215 float unmatchedArea = 0;
1217 it1 != sourcePlanes.end(); it1++)
1218 unmatchedArea += subgraphSrc->pPBM->vPlanes[*it1].areaHull;
1223 while (!sourcePlanes.empty())
1227 cout <<
"Compare " << *it1
1228 <<
" Compare Compare Compare Compare Compare " << endl;
1231 if ((matched.size() +
min(sourcePlanes.size(), targetPlanes.size())) <=
1232 configLocaliser.min_planes_recognition)
1233 if ((matchedArea + unmatchedArea) < areaWinnerMatch)
1236 cout <<
"End branch recursive search. Too short "
1237 << matched.size() <<
" prev winner " << winnerMatch.size()
1255 it2 != targetPlanes.end(); it2++)
1258 cout <<
" " << *it1 <<
" with " << *it2 << endl;
1292 if (hashUnaryConstraints[*it1][*it2] != 1)
1297 bool binaryFail =
false;
1299 it_matched != matched.end(); it_matched++)
1300 if (!evalBinaryConstraints(
1301 subgraphSrc->pPBM->vPlanes[*it1],
1302 subgraphSrc->pPBM->vPlanes[it_matched->first],
1303 subgraphTrg->pPBM->vPlanes[*it2],
1304 subgraphTrg->pPBM->vPlanes[it_matched->second]))
1309 if (binaryFail)
continue;
1314 set<unsigned> nextSrcPlanes = sourcePlanes;
1315 nextSrcPlanes.erase(*it1);
1316 set<unsigned> nextTrgPlanes = targetPlanes;
1317 nextTrgPlanes.erase(*it2);
1318 map<unsigned, unsigned> nextMatched = matched;
1319 nextMatched[*it1] = *it2;
1321 alreadyExplored.push_back(nextMatched);
1323 exploreSubgraphTreeR_Area(
1324 nextSrcPlanes, nextTrgPlanes, nextMatched);
1330 sourcePlanes.erase(it1);
1334 float areaMatched = calcAreaMatched(matched);
1335 if (areaMatched > areaWinnerMatch)
1338 cout <<
"End branch recursive search. matched " << matched.size()
1339 <<
" A " << areaMatched <<
" prev winner " << winnerMatch.size()
1340 <<
" A " << areaWinnerMatch << endl;
1342 areaWinnerMatch = areaMatched;
1343 winnerMatch = matched;
1347 float SubgraphMatcher::calcAreaMatched(
1348 std::map<unsigned, unsigned>& matched_planes)
1350 float areaMatched = 0;
1352 it != matched_planes.end(); it++)
1353 areaMatched += subgraphSrc->pPBM->vPlanes[it->first].areaHull;
1358 float SubgraphMatcher::calcAreaUnmatched(std::set<unsigned>& unmatched_planes)
1360 float areaUnatched = 0;
1362 it != unmatched_planes.end(); it++)
1363 areaUnatched += subgraphSrc->pPBM->vPlanes[*it].areaHull;
1365 return areaUnatched;
1403 std::map<unsigned, unsigned> SubgraphMatcher::compareSubgraphs(
1407 subgraphSrc = &subgraphSource;
1408 subgraphTrg = &subgraphTarget;
1409 map<unsigned, unsigned> matched;
1411 areaWinnerMatch = 0;
1412 winnerMatch.clear();
1413 alreadyExplored.clear();
1416 std::set<unsigned> targetPlanes = subgraphTrg->subgraphPlanesIdx;
1419 cout <<
"Source planes: ";
1421 it2 != sourcePlanes.end(); it2++)
1422 cout <<
" " << *it2;
1424 cout <<
"Target planes: ";
1426 it2 != targetPlanes.end(); it2++)
1427 cout <<
" " << *it2;
1432 hashUnaryConstraints = std::vector<std::vector<int8_t>>(
1433 subgraphSrc->pPBM->vPlanes.size(),
1434 std::vector<int8_t>(subgraphTrg->pPBM->vPlanes.size()));
1437 it1 != sourcePlanes.end(); it1++)
1439 it2 != targetPlanes.end(); it2++)
1440 hashUnaryConstraints[*it1][*it2] =
1441 (evalUnaryConstraints(
1442 subgraphSrc->pPBM->vPlanes[*it1],
1443 subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1447 else if (option == 1)
1449 it1 != sourcePlanes.end(); it1++)
1451 it2 != targetPlanes.end(); it2++)
1452 hashUnaryConstraints[*it1][*it2] =
1453 (evalUnaryConstraintsOdometry(
1454 subgraphSrc->pPBM->vPlanes[*it1],
1455 subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1459 else if (option == 2)
1462 it1 != sourcePlanes.end(); it1++)
1464 it2 != targetPlanes.end(); it2++)
1465 hashUnaryConstraints[*it1][*it2] =
1466 (evalUnaryConstraints2D(
1467 subgraphSrc->pPBM->vPlanes[*it1],
1468 subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1472 else if (option == 3)
1475 it1 != sourcePlanes.end(); it1++)
1477 it2 != targetPlanes.end(); it2++)
1478 hashUnaryConstraints[*it1][*it2] =
1479 (evalUnaryConstraintsOdometry2D(
1480 subgraphSrc->pPBM->vPlanes[*it1],
1481 subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1486 exploreSubgraphTreeR(sourcePlanes, targetPlanes, matched);
1489 cout <<
"Area winnerMatch " << areaWinnerMatch << endl;