From d1958226824a351c428a8091376a65ffa243e64f Mon Sep 17 00:00:00 2001 From: Souriya Trinh Date: Sun, 22 Dec 2024 14:40:55 +0100 Subject: [PATCH] Replace pointers with vectors. Avoid exception "Exception: basic_ios::clear: iostream error" when loading a CAO model without new line at the end of the file. Add sanity checks when loading CAO models. --- cmake/VISPUtils.cmake | 6 +- modules/tracker/mbt/src/vpMbTracker.cpp | 141 +++++----- .../catchGenericTrackerCAOParsing.cpp | 252 ++++++++++++++++++ 3 files changed, 328 insertions(+), 71 deletions(-) diff --git a/cmake/VISPUtils.cmake b/cmake/VISPUtils.cmake index 3c5f06a62e..0afdb10675 100644 --- a/cmake/VISPUtils.cmake +++ b/cmake/VISPUtils.cmake @@ -1849,7 +1849,11 @@ function(vp_find_dataset found location version major minor patch) # Check version if(_found) - if(EXISTS "${_location}/circle/circle.png") + if(EXISTS "${_location}/mbt-cao/circle_model.cao") + set(_major "3") + set(_minor "7") + set(_patch "0") + elseif(EXISTS "${_location}/circle/circle.png") set(_major "3") set(_minor "6") set(_patch "0") diff --git a/modules/tracker/mbt/src/vpMbTracker.cpp b/modules/tracker/mbt/src/vpMbTracker.cpp index bd173a948a..f75114b2af 100644 --- a/modules/tracker/mbt/src/vpMbTracker.cpp +++ b/modules/tracker/mbt/src/vpMbTracker.cpp @@ -1679,6 +1679,8 @@ std::map vpMbTracker::parseParameters(std::string &end void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector &vectorOfModelFilename, int &startIdFace, bool verbose, bool parent, const vpHomogeneousMatrix &odTo) { + const unsigned int maxDataCAO = 100000; // arbitrary value + std::ifstream fileId; fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit); fileId.open(modelFile.c_str(), std::ifstream::in); @@ -1845,17 +1847,14 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector " << caoNbrPoint << " points" << std::endl; } - if (caoNbrPoint > 100000) { + if (caoNbrPoint > maxDataCAO) { throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model."); } if (caoNbrPoint == 0 && !header) { throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined"); } - vpPoint *caoPoints = new vpPoint[caoNbrPoint]; - - int i; // image coordinate (used for matching) - int j; + std::vector caoPoints(caoNbrPoint); for (unsigned int k = 0; k < caoNbrPoint; k++) { removeComment(fileId); @@ -1866,6 +1865,8 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector> pt_3d[2]; if (caoVersion == 2) { + // glob values (not used in MBT but for matching) + int i, j; // image coordinate (used for matching) fileId >> i; fileId >> j; } @@ -1886,7 +1887,7 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector::max(), fileId.widen('\n')); // skip the rest of the line nbLines += caoNbrLine; - unsigned int *caoLinePoints = nullptr; + std::vector caoLinePoints; if (verbose || (parent && !header)) { #if defined(VISP_HAVE_THREADS) std::lock_guard lock(g_mutex_cout); @@ -1894,13 +1895,12 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector " << caoNbrLine << " lines" << std::endl; } - if (caoNbrLine > 100000) { - delete[] caoPoints; + if (caoNbrLine > maxDataCAO) { throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model."); } if (caoNbrLine > 0) - caoLinePoints = new unsigned int[2 * caoNbrLine]; + caoLinePoints.resize(2 * caoNbrLine); unsigned int index1, index2; // Initialization of idFace with startIdFace for dealing with recursive @@ -1976,9 +1976,7 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector " << caoNbrPolygonLine << " polygon lines" << std::endl; } - if (caoNbrPolygonLine > 100000) { - delete[] caoPoints; - delete[] caoLinePoints; + if (caoNbrPolygonLine > maxDataCAO) { throw vpException(vpException::badValue, "Exceed the max number of polygon lines."); } @@ -1989,7 +1987,7 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector> nbLinePol; std::vector corners; - if (nbLinePol > 100000) { + if (nbLinePol > maxDataCAO) { throw vpException(vpException::badValue, "Exceed the max number of lines."); } @@ -2065,7 +2063,7 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector " << caoNbrPolygonPoint << " polygon points" << std::endl; } - if (caoNbrPolygonPoint > 100000) { + if (caoNbrPolygonPoint > maxDataCAO) { throw vpException(vpException::badValue, "Exceed the max number of polygon point."); } @@ -2074,7 +2072,7 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector> nbPointPol; - if (nbPointPol > 100000) { + if (nbPointPol > maxDataCAO) { throw vpException(vpException::badValue, "Exceed the max number of points."); } std::vector corners; @@ -2121,8 +2119,6 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector " << caoNbCylinder << " cylinders" << std::endl; } - if (caoNbCylinder > 100000) { + if (caoNbCylinder > maxDataCAO) { throw vpException(vpException::badValue, "Exceed the max number of cylinders."); } @@ -2203,68 +2199,76 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector> caoNbCircle; - fileId.ignore(std::numeric_limits::max(), fileId.widen('\n')); // skip the rest of the line - - nbCircles += caoNbCircle; - if (verbose || (parent && !header)) { + // To handle CAO model file without no new line at the end, we check if the first char is zero or not + // Otherwise "fileId >> caoNbCircle;" gives: + // "Cannot get the number of circles. Defaulting to zero." + // "Exception: basic_ios::clear: iostream error" + char c_circle; + fileId.get(c_circle); + int nb_circles = c_circle - '0'; + if (nb_circles > 0) { + fileId.unget(); + + /* Extract the circles */ + fileId >> caoNbCircle; + fileId.ignore(std::numeric_limits::max(), fileId.widen('\n')); // skip the rest of the line + + nbCircles += caoNbCircle; + if (verbose || (parent && !header)) { #if defined(VISP_HAVE_THREADS) - std::lock_guard lock(g_mutex_cout); + std::lock_guard lock(g_mutex_cout); #endif - std::cout << "> " << caoNbCircle << " circles" << std::endl; - } + std::cout << "> " << caoNbCircle << " circles" << std::endl; + } - if (caoNbCircle > 100000) { - throw vpException(vpException::badValue, "Exceed the max number of cicles."); - } + if (caoNbCircle > maxDataCAO) { + throw vpException(vpException::badValue, "Exceed the max number of cicles."); + } - for (unsigned int k = 0; k < caoNbCircle; ++k) { - removeComment(fileId); + for (unsigned int k = 0; k < caoNbCircle; ++k) { + removeComment(fileId); + + double radius; + unsigned int indexP1, indexP2, indexP3; + fileId >> radius; + fileId >> indexP1; + fileId >> indexP2; + fileId >> indexP3; + + //////////////////////////Read the parameter value if present////////////////////////// + // Get the end of the line + std::string endLine = ""; + if (safeGetline(fileId, endLine).good()) { + std::map mapOfParams = parseParameters(endLine); + + std::string polygonName = ""; + bool useLod = !applyLodSettingInConfig ? useLodGeneral : false; + double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0; + if (mapOfParams.find("name") != mapOfParams.end()) { + polygonName = mapOfParams["name"]; + } + if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) { + minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str()); + } + if (mapOfParams.find("useLod") != mapOfParams.end()) { + useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]); + } - double radius; - unsigned int indexP1, indexP2, indexP3; - fileId >> radius; - fileId >> indexP1; - fileId >> indexP2; - fileId >> indexP3; + addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod, + minPolygonAreaThreshold); - //////////////////////////Read the parameter value if present////////////////////////// - // Get the end of the line - std::string endLine = ""; - if (safeGetline(fileId, endLine).good()) { - std::map mapOfParams = parseParameters(endLine); + initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName); - std::string polygonName = ""; - bool useLod = !applyLodSettingInConfig ? useLodGeneral : false; - double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0; - if (mapOfParams.find("name") != mapOfParams.end()) { - polygonName = mapOfParams["name"]; + addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, + polygonName, useLod, minPolygonAreaThreshold); + initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++, + polygonName); } - if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) { - minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str()); - } - if (mapOfParams.find("useLod") != mapOfParams.end()) { - useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]); - } - - addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod, - minPolygonAreaThreshold); - - initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName); - - addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, - polygonName, useLod, minPolygonAreaThreshold); - initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++, - polygonName); } } - } catch (const std::exception &e) { std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl; @@ -2274,9 +2278,6 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector= 0x030700) +static const double margin = 1e-9; + +static void printFacesInfo(vpMbHiddenFaces &faces) +{ + std::cout << "Number of faces: " << faces.size() << std::endl; + + for (unsigned int i = 0; i < faces.size(); i++) { + std::vector &poly = faces.getPolygon(); + std::cout << "face " << i << " with index: " << poly[i]->getIndex() + << (poly[i]->getName().empty() ? "" : (" with name: " + poly[i]->getName())) + << " is " << (poly[i]->isVisible() ? "visible" : "not visible") + << " and has " << poly[i]->getNbPoint() << " points" + << " and LOD is " << (poly[i]->useLod ? "enabled" : "disabled") << std::endl; + + for (unsigned int j = 0; j < poly[i]->getNbPoint(); j++) { + vpPoint P = poly[i]->getPoint(j); + std::cout << " P obj " << j << ": " << P.get_oX() << " " << P.get_oY() << " " << P.get_oZ() << std::endl; + } + } +} + +TEST_CASE("vpMbGenericTracker load CAO model [lines]", "[vpMbGenericTracker CAO parsing]") +{ + const std::string cao_filename = + vpIoTools::createFilePath(ipath, "mbt-cao/line_model.cao"); + vpMbGenericTracker tracker; + const bool verbose = true; + REQUIRE_NOTHROW(tracker.loadModel(cao_filename, verbose)); + + std::list lines; + tracker.getLline(lines); + REQUIRE(lines.size() == 2); + + int idx = 0; + std::vector line_names = { "line_0", "line_1" }; + for (auto line : lines) { + CHECK_THAT(line->p1->get_oX(), Catch::Matchers::WithinAbs(idx*3 + 0, margin)); + CHECK_THAT(line->p1->get_oY(), Catch::Matchers::WithinAbs(idx*3 + 1, margin)); + CHECK_THAT(line->p1->get_oZ(), Catch::Matchers::WithinAbs(idx*3 + 2, margin)); + CHECK(line->getName() == line_names[idx]); + ++idx; + } +} + +TEST_CASE("vpMbGenericTracker load CAO model [face from lines]", "[vpMbGenericTracker CAO parsing]") +{ + const std::string cao_filename = + vpIoTools::createFilePath(ipath, "mbt-cao/face_from_lines_model.cao"); + vpMbGenericTracker tracker; + const bool verbose = true; + REQUIRE_NOTHROW(tracker.loadModel(cao_filename, verbose)); + + std::list lines; + tracker.getLline(lines); + CHECK(lines.size() == 3); + + vpMbHiddenFaces &faces = tracker.getFaces(); + CHECK(faces.size() == 1); + + for (unsigned int i = 0; i < faces.size(); i++) { + const std::vector &poly = faces.getPolygon(); + CHECK(poly[i]->getNbPoint() == 4); + CHECK(poly[i]->getName() == "face_from_3D_lines"); + + for (unsigned int j = 0; j < poly[i]->getNbPoint(); j++) { + vpPoint P = poly[i]->getPoint(j); + if (j == 3) { + CHECK_THAT(P.get_oX(), Catch::Matchers::WithinAbs(0, margin)); + CHECK_THAT(P.get_oY(), Catch::Matchers::WithinAbs(1, margin)); + CHECK_THAT(P.get_oZ(), Catch::Matchers::WithinAbs(2, margin)); + } + else { + CHECK_THAT(P.get_oX(), Catch::Matchers::WithinAbs(j*3 + 0, margin)); + CHECK_THAT(P.get_oY(), Catch::Matchers::WithinAbs(j*3 + 1, margin)); + CHECK_THAT(P.get_oZ(), Catch::Matchers::WithinAbs(j*3 + 2, margin)); + } + } + } +} + +TEST_CASE("vpMbGenericTracker load CAO model [face from points]", "[vpMbGenericTracker CAO parsing]") +{ + const std::string cao_filename = + vpIoTools::createFilePath(ipath, "mbt-cao/face_from_points_model.cao"); + vpMbGenericTracker tracker; + const bool verbose = true; + REQUIRE_NOTHROW(tracker.loadModel(cao_filename, verbose)); + + std::list lines; + tracker.getLline(lines); + CHECK(lines.size() == 3); + + vpMbHiddenFaces &faces = tracker.getFaces(); + CHECK(faces.size() == 1); + + for (unsigned int i = 0; i < faces.size(); i++) { + const std::vector &poly = faces.getPolygon(); + CHECK(poly[i]->getNbPoint() == 3); + CHECK(poly[i]->getName() == "face_from_3D_points"); + + for (unsigned int j = 0; j < poly[i]->getNbPoint(); j++) { + vpPoint P = poly[i]->getPoint(j); + CHECK_THAT(P.get_oX(), Catch::Matchers::WithinAbs(j*3 + 0, margin)); + CHECK_THAT(P.get_oY(), Catch::Matchers::WithinAbs(j*3 + 1, margin)); + CHECK_THAT(P.get_oZ(), Catch::Matchers::WithinAbs(j*3 + 2, margin)); + } + } +} + +TEST_CASE("vpMbGenericTracker load CAO model [cylinder]", "[vpMbGenericTracker CAO parsing]") +{ + const std::string cao_filename = + vpIoTools::createFilePath(ipath, "mbt-cao/cylinder_model.cao"); + vpMbGenericTracker tracker; + const bool verbose = true; + REQUIRE_NOTHROW(tracker.loadModel(cao_filename, verbose)); + + std::list cylinders; + tracker.getLcylinder(cylinders); + CHECK(cylinders.size() == 1); + + vpMbtDistanceCylinder *cylinder = cylinders.front(); + CHECK(cylinder->getName() == "cylinder"); + CHECK_THAT(cylinder->radius, Catch::Matchers::WithinAbs(0.5, margin)); + + CHECK_THAT(cylinder->p1->get_oX(), Catch::Matchers::WithinAbs(0, margin)); + CHECK_THAT(cylinder->p1->get_oY(), Catch::Matchers::WithinAbs(1, margin)); + CHECK_THAT(cylinder->p1->get_oZ(), Catch::Matchers::WithinAbs(2, margin)); + + CHECK_THAT(cylinder->p2->get_oX(), Catch::Matchers::WithinAbs(3, margin)); + CHECK_THAT(cylinder->p2->get_oY(), Catch::Matchers::WithinAbs(4, margin)); + CHECK_THAT(cylinder->p2->get_oZ(), Catch::Matchers::WithinAbs(5, margin)); + + vpMbHiddenFaces &faces = tracker.getFaces(); + CHECK(faces.size() == 5); + printFacesInfo(faces); +} + +TEST_CASE("vpMbGenericTracker load CAO model [circle]", "[vpMbGenericTracker CAO parsing]") +{ + const std::string cao_filename = + vpIoTools::createFilePath(ipath, "mbt-cao/circle_model.cao"); + vpMbGenericTracker tracker; + const bool verbose = true; + REQUIRE_NOTHROW(tracker.loadModel(cao_filename, verbose)); + + std::list circles; + tracker.getLcircle(circles); + CHECK(circles.size() == 1); + + vpMbtDistanceCircle *circle = circles.front(); + CHECK(circle->getName() == "circle"); + CHECK_THAT(circle->radius, Catch::Matchers::WithinAbs(0.5, margin)); + + CHECK_THAT(circle->p1->get_oX(), Catch::Matchers::WithinAbs(0, margin)); + CHECK_THAT(circle->p1->get_oY(), Catch::Matchers::WithinAbs(1, margin)); + CHECK_THAT(circle->p1->get_oZ(), Catch::Matchers::WithinAbs(2, margin)); + + CHECK_THAT(circle->p2->get_oX(), Catch::Matchers::WithinAbs(3, margin)); + CHECK_THAT(circle->p2->get_oY(), Catch::Matchers::WithinAbs(4, margin)); + CHECK_THAT(circle->p2->get_oZ(), Catch::Matchers::WithinAbs(5, margin)); + + CHECK_THAT(circle->p3->get_oX(), Catch::Matchers::WithinAbs(6, margin)); + CHECK_THAT(circle->p3->get_oY(), Catch::Matchers::WithinAbs(7, margin)); + CHECK_THAT(circle->p3->get_oZ(), Catch::Matchers::WithinAbs(8, margin)); + + vpMbHiddenFaces &faces = tracker.getFaces(); + CHECK(faces.size() == 1); + printFacesInfo(faces); +} + +TEST_CASE("vpMbGenericTracker load CAO model [hierarchical]", "[vpMbGenericTracker CAO parsing]") +{ + const std::string cao_filename = + vpIoTools::createFilePath(ipath, "mbt-cao/hierarchical_model.cao"); + vpMbGenericTracker tracker; + const bool verbose = true; + REQUIRE_NOTHROW(tracker.loadModel(cao_filename, verbose)); + + // Lines + { + std::list lines; + tracker.getLline(lines); + REQUIRE(lines.size() == 2); + std::vector line_names = { "line_0", "line_1" }; + + int idx = 0; + for (auto line : lines) { + CHECK_THAT(line->p1->get_oX(), Catch::Matchers::WithinAbs(idx*3 + 0, margin)); + CHECK_THAT(line->p1->get_oY(), Catch::Matchers::WithinAbs(idx*3 + 1, margin)); + CHECK_THAT(line->p1->get_oZ(), Catch::Matchers::WithinAbs(idx*3 + 2, margin)); + CHECK(line->getName() == line_names[idx]); + ++idx; + } + } + + // Face from points + { + vpMbHiddenFaces &faces = tracker.getFaces(); + // vpMbtDistanceLine --> 2 faces + // vpMbtDistanceCylinder --> 1 + 4 faces + // vpMbtDistanceCircle --> 1 face + CHECK(faces.size() == 8); + printFacesInfo(faces); + } + + // Cylinder + { + std::list cylinders; + tracker.getLcylinder(cylinders); + CHECK(cylinders.size() == 1); + + vpMbtDistanceCylinder *cylinder = cylinders.front(); + CHECK(cylinder->getName() == "cylinder"); + CHECK_THAT(cylinder->radius, Catch::Matchers::WithinAbs(0.5, margin)); + + CHECK_THAT(cylinder->p1->get_oX(), Catch::Matchers::WithinAbs(0, margin)); + CHECK_THAT(cylinder->p1->get_oY(), Catch::Matchers::WithinAbs(1, margin)); + CHECK_THAT(cylinder->p1->get_oZ(), Catch::Matchers::WithinAbs(2, margin)); + + CHECK_THAT(cylinder->p2->get_oX(), Catch::Matchers::WithinAbs(3, margin)); + CHECK_THAT(cylinder->p2->get_oY(), Catch::Matchers::WithinAbs(4, margin)); + CHECK_THAT(cylinder->p2->get_oZ(), Catch::Matchers::WithinAbs(5, margin)); + } + + // Circle + { + std::list circles; + tracker.getLcircle(circles); + CHECK(circles.size() == 1); + + vpMbtDistanceCircle *circle = circles.front(); + CHECK(circle->getName() == "circle"); + CHECK_THAT(circle->radius, Catch::Matchers::WithinAbs(0.5, margin)); + + CHECK_THAT(circle->p1->get_oX(), Catch::Matchers::WithinAbs(0, margin)); + CHECK_THAT(circle->p1->get_oY(), Catch::Matchers::WithinAbs(1, margin)); + CHECK_THAT(circle->p1->get_oZ(), Catch::Matchers::WithinAbs(2, margin)); + + CHECK_THAT(circle->p2->get_oX(), Catch::Matchers::WithinAbs(3, margin)); + CHECK_THAT(circle->p2->get_oY(), Catch::Matchers::WithinAbs(4, margin)); + CHECK_THAT(circle->p2->get_oZ(), Catch::Matchers::WithinAbs(5, margin)); + + CHECK_THAT(circle->p3->get_oX(), Catch::Matchers::WithinAbs(6, margin)); + CHECK_THAT(circle->p3->get_oY(), Catch::Matchers::WithinAbs(7, margin)); + CHECK_THAT(circle->p3->get_oZ(), Catch::Matchers::WithinAbs(8, margin)); + } +} + +#endif + int main(int argc, char *argv[]) { Catch::Session session;