50 #include <visp3/core/vpColVector.h>
51 #include <visp3/core/vpDisplay.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/core/vpMatrix.h>
54 #include <visp3/core/vpPoint.h>
55 #include <visp3/vision/vpPose.h>
56 #ifdef VISP_HAVE_MODULE_GUI
57 #include <visp3/gui/vpDisplayGDI.h>
58 #include <visp3/gui/vpDisplayOpenCV.h>
59 #include <visp3/gui/vpDisplayX.h>
61 #include <visp3/core/vpCameraParameters.h>
62 #include <visp3/core/vpColor.h>
63 #include <visp3/core/vpException.h>
64 #include <visp3/core/vpIoTools.h>
65 #include <visp3/core/vpPixelMeterConversion.h>
66 #ifdef VISP_HAVE_MODULE_IO
67 #include <visp3/io/vpImageIo.h>
69 #include <visp3/core/vpCPUFeatures.h>
70 #include <visp3/core/vpIoTools.h>
71 #include <visp3/core/vpMatrixException.h>
72 #include <visp3/core/vpTrackingException.h>
73 #include <visp3/mbt/vpMbTracker.h>
75 #include <visp3/core/vpImageFilter.h>
76 #include <visp3/mbt/vpMbtXmlGenericParser.h>
78 #ifdef VISP_HAVE_COIN3D
80 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
81 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
82 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
83 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
84 #include <Inventor/VRMLnodes/SoVRMLShape.h>
85 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
86 #include <Inventor/actions/SoGetMatrixAction.h>
87 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
88 #include <Inventor/actions/SoSearchAction.h>
89 #include <Inventor/actions/SoToVRML2Action.h>
90 #include <Inventor/actions/SoWriteAction.h>
91 #include <Inventor/misc/SoChildList.h>
92 #include <Inventor/nodes/SoSeparator.h>
95 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
96 #include <emmintrin.h>
97 #define VISP_HAVE_SSE2 1
100 #ifndef DOXYGEN_SHOULD_SKIP_THIS
108 SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
110 std::vector<vpPoint> extremities;
113 double minLineLengthThresh;
120 struct PolygonFaceInfo {
121 PolygonFaceInfo(
double dist,
const vpPolygon &poly,
const std::vector<vpPoint> &corners)
122 : distanceToCamera(dist), polygon(poly), faceCorners(corners)
126 bool operator<(
const PolygonFaceInfo &pfi)
const {
return distanceToCamera < pfi.distanceToCamera; }
128 double distanceToCamera;
130 std::vector<vpPoint> faceCorners;
133 #endif // DOXYGEN_SHOULD_SKIP_THIS
141 : m_cam(), m_cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
142 computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
143 displayFeatures(false), m_optimizationMethod(
vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(
vpMath::rad(89)),
144 angleDisappears(
vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(
vpPolygon3D::NO_CLIPPING),
145 useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
146 nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
147 minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
148 m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
149 m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(),
150 m_projectionErrorFaces(), m_projectionErrorOgreShowConfigDialog(false),
151 m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5,5), m_SobelY(5,5),
152 m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), m_projectionErrorDisplayThickness(1),
153 m_projectionErrorCam(), m_mask(NULL), m_I()
158 mapOfParameterNames[
"name"] =
"string";
159 mapOfParameterNames[
"minPolygonAreaThreshold"] =
"number";
160 mapOfParameterNames[
"minLineLengthThreshold"] =
"number";
161 mapOfParameterNames[
"useLod"] =
"boolean";
190 #ifdef VISP_HAVE_MODULE_GUI
199 std::string ext =
".init";
200 std::string str_pose =
"";
201 size_t pos = initFile.rfind(ext);
204 std::fstream finitpos;
206 char s[FILENAME_MAX];
208 if (pos != std::string::npos)
209 str_pose = initFile.substr(0, pos) +
".0.pos";
211 str_pose = initFile +
".0.pos";
213 finitpos.open(str_pose.c_str(), std::ios::in);
214 sprintf(s,
"%s", str_pose.c_str());
219 if (finitpos.fail()) {
220 std::cout <<
"cannot read " << s << std::endl <<
"cMo set to identity" << std::endl;
223 for (
unsigned int i = 0; i < 6; i += 1) {
224 finitpos >> init_pos[i];
230 std::cout <<
"last_cMo : " << std::endl << last_cMo << std::endl;
244 std::cout <<
"No modification : left click " << std::endl;
245 std::cout <<
"Modify initial pose : right click " << std::endl;
287 if (pos != std::string::npos)
288 sprintf(s,
"%s", initFile.c_str());
290 sprintf(s,
"%s.init", initFile.c_str());
292 std::cout <<
"Load 3D points from: " << s << std::endl;
295 std::cout <<
"cannot read " << s << std::endl;
299 #ifdef VISP_HAVE_MODULE_IO
303 const std::string imgExtVec[] = {
".ppm",
".pgm",
".jpg",
".jpeg",
".png"};
305 bool foundHelpImg =
false;
306 if (pos != std::string::npos) {
307 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
308 dispF = initFile.substr(0, pos) + imgExtVec[i];
312 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
313 dispF = initFile + imgExtVec[i];
319 std::cout <<
"Load image to help initialization: " << dispF << std::endl;
320 #if defined VISP_HAVE_X11
322 #elif defined VISP_HAVE_GDI
324 #elif defined VISP_HAVE_OPENCV
330 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
334 d_help->init(Iref, winXPos + (
int)width + 80, winYPos,
335 "Where to initialize...");
342 if (d_help != NULL) {
347 #else //#ifdef VISP_HAVE_MODULE_IO
349 #endif //#ifdef VISP_HAVE_MODULE_IO
355 finit.ignore(256,
'\n');
356 std::cout <<
"Number of 3D points " << n3d << std::endl;
361 std::vector<vpPoint> P(n3d);
362 for (
unsigned int i = 0; i < n3d; i++) {
370 finit.ignore(256,
'\n');
373 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << pt_3d_tf[0] <<
" " << pt_3d_tf[1] <<
" " << pt_3d_tf[2] << std::endl;
375 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
380 bool isWellInit =
false;
381 while (!isWellInit) {
382 std::vector<vpImagePoint> mem_ip;
383 for (
unsigned int i = 0; i < n3d; i++) {
384 std::ostringstream text;
385 text <<
"Click on point " << i + 1;
389 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
396 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
402 std::cout <<
"Click on point " << i + 1 <<
" ";
406 mem_ip.push_back(ip);
410 mem_ip.push_back(ip);
417 std::cout <<
"with 2D coordinates: " << ip << std::endl;
431 d1 = d2 = std::numeric_limits<double>::max();
501 if (d_help != NULL) {
507 std::cout <<
"cMo : " << std::endl <<
m_cMo << std::endl;
551 initClick(&I, NULL, initFile, displayHelp, T);
588 initClick(NULL, &I_color, initFile, displayHelp, T);
592 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile)
605 std::vector<vpPoint> P;
606 for (
unsigned int i = 0; i < points3D_list.size(); i++)
607 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
609 #ifdef VISP_HAVE_MODULE_IO
614 std::cout <<
"Load image to help initialization: " << displayFile << std::endl;
615 #if defined VISP_HAVE_X11
617 #elif defined VISP_HAVE_GDI
619 #elif defined VISP_HAVE_OPENCV
624 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
627 "Where to initialize...");
630 "Where to initialize...");
636 if (d_help != NULL) {
642 #else //#ifdef VISP_HAVE_MODULE_IO
644 #endif //#ifdef VISP_HAVE_MODULE_IO
647 bool isWellInit =
false;
648 while (!isWellInit) {
649 for (
unsigned int i = 0; i < points3D_list.size(); i++) {
650 std::cout <<
"Click on point " << i + 1 << std::endl;
665 std::cout <<
"Click on point " << ip << std::endl;
682 d1 = d2 = std::numeric_limits<double>::max();
748 if (d_help != NULL) {
773 const std::string &displayFile)
775 initClick(&I, NULL, points3D_list, displayFile);
790 const std::string &displayFile)
792 initClick(NULL, &I_color, points3D_list, displayFile);
794 #endif //#ifdef VISP_HAVE_MODULE_GUI
797 const std::string &initFile)
799 char s[FILENAME_MAX];
802 std::string ext =
".init";
803 size_t pos = initFile.rfind(ext);
805 if (pos == initFile.size() - ext.size() && pos != 0)
806 sprintf(s,
"%s", initFile.c_str());
808 sprintf(s,
"%s.init", initFile.c_str());
810 std::cout <<
"Load 2D/3D points from: " << s << std::endl;
811 finit.open(s, std::ios::in);
813 std::cout <<
"cannot read " << s << std::endl;
823 while (!finit.fail() && (c ==
'#')) {
824 finit.ignore(256,
'\n');
831 finit.ignore(256,
'\n');
832 std::cout <<
"Number of 3D points " << n3d << std::endl;
838 for (
unsigned int i = 0; i < n3d; i++) {
841 while (!finit.fail() && (c ==
'#')) {
842 finit.ignore(256,
'\n');
850 finit.ignore(256,
'\n');
852 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << X <<
" " << Y <<
" " << Z << std::endl;
861 while (!finit.fail() && (c ==
'#')) {
862 finit.ignore(256,
'\n');
869 finit.ignore(256,
'\n');
870 std::cout <<
"Number of 2D points " << n2d << std::endl;
879 "In %s file, number of 2D points %d and number of 3D "
880 "points %d are not equal",
885 for (
unsigned int i = 0; i < n2d; i++) {
888 while (!finit.fail() && (c ==
'#')) {
889 finit.ignore(256,
'\n');
893 double u, v, x = 0, y = 0;
896 finit.ignore(256,
'\n');
899 std::cout <<
"Point " << i + 1 <<
" with 2D coordinates: " << ip << std::endl;
910 d1 = d2 = std::numeric_limits<double>::max();
1002 const std::vector<vpImagePoint> &points2D_list,
const std::vector<vpPoint> &points3D_list)
1004 if (points2D_list.size() != points3D_list.size())
1005 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points "
1006 "different to the number of 3D points.");
1008 size_t size = points3D_list.size();
1009 std::vector<vpPoint> P;
1012 for (
size_t i = 0; i < size; i++) {
1013 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1014 double x = 0, y = 0;
1023 d1 = d2 = std::numeric_limits<double>::max();
1063 const std::vector<vpPoint> &points3D_list)
1077 const std::vector<vpPoint> &points3D_list)
1083 const std::string &initFile)
1085 char s[FILENAME_MAX];
1089 std::string ext =
".pos";
1090 size_t pos = initFile.rfind(ext);
1092 if (pos == initFile.size() - ext.size() && pos != 0)
1093 sprintf(s,
"%s", initFile.c_str());
1095 sprintf(s,
"%s.pos", initFile.c_str());
1097 finit.open(s, std::ios::in);
1099 std::cout <<
"cannot read " << s << std::endl;
1103 for (
unsigned int i = 0; i < 6; i += 1) {
1104 finit >> init_pos[i];
1221 std::fstream finitpos;
1222 char s[FILENAME_MAX];
1224 sprintf(s,
"%s", filename.c_str());
1225 finitpos.open(s, std::ios::out);
1228 finitpos << init_pos;
1233 bool useLod,
double minPolygonAreaThreshold,
1234 double minLineLengthThreshold)
1236 std::vector<vpPoint> corners_without_duplicates;
1237 corners_without_duplicates.push_back(corners[0]);
1238 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
1239 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1240 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1241 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1242 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1243 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1244 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1245 corners_without_duplicates.push_back(corners[i + 1]);
1250 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
1272 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1273 polygon.
addPoint(j, corners_without_duplicates[j]);
1289 int idFace,
const std::string &polygonName,
bool useLod,
1290 double minPolygonAreaThreshold)
1323 y[0] = plane.
getA() / norm_Y;
1324 y[1] = plane.
getB() / norm_Y;
1325 y[2] = plane.
getC() / norm_Y;
1328 for (
unsigned int i = 0; i < 3; i++) {
1344 for (
unsigned int i = 0; i < 4; i++) {
1347 w_p = wMc * cMc_90 * c_p;
1370 bool useLod,
double minLineLengthThreshold)
1408 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
1411 for (
unsigned int i = 0; i < listFaces.size(); i++) {
1413 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
1414 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
1415 polygon.
addPoint(j, listFaces[i][j]);
1468 std::string::const_iterator it;
1471 it = modelFile.end();
1472 if ((*(it - 1) ==
'o' && *(it - 2) ==
'a' && *(it - 3) ==
'c' && *(it - 4) ==
'.') ||
1473 (*(it - 1) ==
'O' && *(it - 2) ==
'A' && *(it - 3) ==
'C' && *(it - 4) ==
'.')) {
1474 std::vector<std::string> vectorOfModelFilename;
1482 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose,
true, odTo);
1483 }
else if ((*(it - 1) ==
'l' && *(it - 2) ==
'r' && *(it - 3) ==
'w' && *(it - 4) ==
'.') ||
1484 (*(it - 1) ==
'L' && *(it - 2) ==
'R' && *(it - 3) ==
'W' && *(it - 4) ==
'.')) {
1529 #ifdef VISP_HAVE_COIN3D
1533 SbBool ok = in.openFile(modelFile.c_str());
1534 SoVRMLGroup *sceneGraphVRML2;
1541 if (!in.isFileVRML2()) {
1542 SoSeparator *sceneGraph = SoDB::readAll(&in);
1543 if (sceneGraph == NULL) {
1547 SoToVRML2Action tovrml2;
1548 tovrml2.apply(sceneGraph);
1550 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1551 sceneGraphVRML2->ref();
1552 sceneGraph->unref();
1554 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1555 if (sceneGraphVRML2 == NULL) {
1557 sceneGraphVRML2->ref();
1566 sceneGraphVRML2->unref();
1568 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1578 while (!fileId.fail() && (c ==
'#')) {
1579 fileId.ignore(256,
'\n');
1582 if (fileId.fail()) {
1590 std::map<std::string, std::string> mapOfParams;
1593 while (!endLine.empty() && !exit) {
1599 std::string param(it->first +
"=");
1602 if (endLine.compare(0, param.size(), param) == 0) {
1604 endLine = endLine.substr(param.size());
1606 bool parseQuote =
false;
1607 if (it->second ==
"string") {
1609 if (endLine.size() > 2 && endLine[0] ==
'"') {
1611 endLine = endLine.substr(1);
1612 size_t pos = endLine.find_first_of(
'"');
1614 if (pos != std::string::npos) {
1615 mapOfParams[it->first] = endLine.substr(0, pos);
1616 endLine = endLine.substr(pos + 1);
1626 size_t pos1 = endLine.find_first_of(
' ');
1627 size_t pos2 = endLine.find_first_of(
'\t');
1628 size_t pos = pos1 < pos2 ? pos1 : pos2;
1630 mapOfParams[it->first] = endLine.substr(0, pos);
1631 endLine = endLine.substr(pos + 1);
1690 int &startIdFace,
bool verbose,
bool parent,
1693 std::ifstream fileId;
1694 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1695 fileId.open(modelFile.c_str(), std::ifstream::in);
1696 if (fileId.fail()) {
1697 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
1702 std::cout <<
"Model file : " << modelFile << std::endl;
1704 vectorOfModelFilename.push_back(modelFile);
1717 fileId >> caoVersion;
1718 fileId.ignore(256,
'\n');
1720 std::cout <<
"in vpMbTracker::loadCAOModel() -> Bad parameter header "
1721 "file : use V0, V1, ...";
1723 "header file : use V0, V1, ...");
1730 const std::string prefix_load =
"load";
1734 bool header =
false;
1735 while (c ==
'l' || c ==
'L') {
1736 getline(fileId, line);
1738 if (!line.compare(0, prefix_load.size(), prefix_load)) {
1740 std::string paramsStr = line.substr(5);
1742 paramsStr = paramsStr.substr(0, paramsStr.find_first_of(
")"));
1746 for (
size_t i = 0; i < params.size(); i++) {
1750 if (!params.empty()) {
1752 std::string headerPathRead = params[0];
1753 headerPathRead = headerPathRead.substr(1);
1754 headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of(
"\""));
1756 std::string headerPath = headerPathRead;
1771 for (
size_t i = 1; i < params.size(); i++) {
1772 std::string param = params[i];
1774 const std::string prefix =
"t=[";
1775 if (!param.compare(0, prefix.size(), prefix)) {
1776 param = param.substr(prefix.size());
1777 param = param.substr(0, param.find_first_of(
"]"));
1780 if (values.size() == 3) {
1781 t[0] = atof(values[0].c_str());
1782 t[1] = atof(values[1].c_str());
1783 t[2] = atof(values[2].c_str());
1788 const std::string prefix =
"tu=[";
1789 if (!param.compare(0, prefix.size(), prefix)) {
1790 param = param.substr(prefix.size());
1791 param = param.substr(0, param.find_first_of(
"]"));
1794 if (values.size() == 3) {
1795 for (
size_t j = 0; j < values.size(); j++) {
1796 std::string value = values[j];
1798 size_t unitPos = value.find(
"deg");
1799 if (unitPos != std::string::npos) {
1800 value = value.substr(0, unitPos);
1804 unitPos = value.find(
"rad");
1805 if (unitPos != std::string::npos) {
1806 value = value.substr(0, unitPos);
1808 tu[static_cast<unsigned int>(j)] = !radian ?
vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1816 bool cyclic =
false;
1817 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1818 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1819 if (headerPath == *it) {
1827 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose,
false, odTo*oTo_local);
1832 std::cout <<
"WARNING Cyclic dependency detected with file " << headerPath <<
" declared in " << modelFile
1844 unsigned int caoNbrPoint;
1845 fileId >> caoNbrPoint;
1846 fileId.ignore(256,
'\n');
1849 if (verbose || (parent && !header)) {
1850 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
1853 if (caoNbrPoint > 100000) {
1857 if (caoNbrPoint == 0 && !header) {
1865 for (
unsigned int k = 0; k < caoNbrPoint; k++) {
1873 if (caoVersion == 2) {
1878 fileId.ignore(256,
'\n');
1888 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1889 unsigned int caoNbrLine;
1890 fileId >> caoNbrLine;
1891 fileId.ignore(256,
'\n');
1894 unsigned int *caoLinePoints = NULL;
1895 if (verbose || (parent && !header)) {
1896 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
1899 if (caoNbrLine > 100000) {
1905 caoLinePoints =
new unsigned int[2 * caoNbrLine];
1907 unsigned int index1, index2;
1910 int idFace = startIdFace;
1912 for (
unsigned int k = 0; k < caoNbrLine; k++) {
1921 fileId.getline(buffer, 256);
1922 std::string endLine(buffer);
1923 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1925 std::string segmentName =
"";
1928 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1929 segmentName = mapOfParams[
"name"];
1931 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1932 minLineLengthThresh = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1934 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1938 SegmentInfo segmentInfo;
1939 segmentInfo.name = segmentName;
1940 segmentInfo.useLod = useLod;
1941 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1943 caoLinePoints[2 * k] = index1;
1944 caoLinePoints[2 * k + 1] = index2;
1946 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1947 std::vector<vpPoint> extremities;
1948 extremities.push_back(caoPoints[index1]);
1949 extremities.push_back(caoPoints[index2]);
1950 segmentInfo.extremities = extremities;
1952 std::pair<unsigned int, unsigned int> key(index1, index2);
1954 segmentTemporaryMap[key] = segmentInfo;
1956 vpTRACE(
" line %d has wrong coordinates.", k);
1967 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1968 unsigned int caoNbrPolygonLine;
1969 fileId >> caoNbrPolygonLine;
1970 fileId.ignore(256,
'\n');
1973 if (verbose || (parent && !header)) {
1974 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon lines" << std::endl;
1977 if (caoNbrPolygonLine > 100000) {
1979 delete[] caoLinePoints;
1984 for (
unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1987 unsigned int nbLinePol;
1988 fileId >> nbLinePol;
1989 std::vector<vpPoint> corners;
1990 if (nbLinePol > 100000) {
1994 for (
unsigned int n = 0; n < nbLinePol; n++) {
1997 if (index >= caoNbrLine) {
2000 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2001 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2003 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2004 faceSegmentKeyVector.push_back(key);
2010 fileId.getline(buffer, 256);
2011 std::string endLine(buffer);
2012 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2014 std::string polygonName =
"";
2017 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2018 polygonName = mapOfParams[
"name"];
2020 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2021 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2023 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2035 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2036 it != segmentTemporaryMap.end(); ++it) {
2037 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2038 faceSegmentKeyVector.end()) {
2040 it->second.minLineLengthThresh);
2044 it->second.minLineLengthThresh);
2053 unsigned int caoNbrPolygonPoint;
2054 fileId >> caoNbrPolygonPoint;
2055 fileId.ignore(256,
'\n');
2058 if (verbose || (parent && !header)) {
2059 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon points" << std::endl;
2062 if (caoNbrPolygonPoint > 100000) {
2066 for (
unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2069 unsigned int nbPointPol;
2070 fileId >> nbPointPol;
2071 if (nbPointPol > 100000) {
2074 std::vector<vpPoint> corners;
2075 for (
unsigned int n = 0; n < nbPointPol; n++) {
2077 if (index > caoNbrPoint - 1) {
2080 corners.push_back(caoPoints[index]);
2086 fileId.getline(buffer, 256);
2087 std::string endLine(buffer);
2088 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2090 std::string polygonName =
"";
2093 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2094 polygonName = mapOfParams[
"name"];
2096 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2097 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2099 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2111 unsigned int caoNbCylinder;
2118 delete[] caoLinePoints;
2123 fileId >> caoNbCylinder;
2124 fileId.ignore(256,
'\n');
2127 if (verbose || (parent && !header)) {
2128 std::cout <<
"> " << caoNbCylinder <<
" cylinders" << std::endl;
2131 if (caoNbCylinder > 100000) {
2135 for (
unsigned int k = 0; k < caoNbCylinder; ++k) {
2139 unsigned int indexP1, indexP2;
2147 fileId.getline(buffer, 256);
2148 std::string endLine(buffer);
2149 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2151 std::string polygonName =
"";
2154 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2155 polygonName = mapOfParams[
"name"];
2157 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
2158 minLineLengthThreshold = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
2160 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2164 int idRevolutionAxis = idFace;
2165 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2167 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
2169 std::vector<std::vector<vpPoint> > listFaces;
2171 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2173 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2182 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2187 unsigned int caoNbCircle;
2194 delete[] caoLinePoints;
2199 fileId >> caoNbCircle;
2200 fileId.ignore(256,
'\n');
2203 if (verbose || (parent && !header)) {
2204 std::cout <<
"> " << caoNbCircle <<
" circles" << std::endl;
2207 if (caoNbCircle > 100000) {
2211 for (
unsigned int k = 0; k < caoNbCircle; ++k) {
2215 unsigned int indexP1, indexP2, indexP3;
2224 fileId.getline(buffer, 256);
2225 std::string endLine(buffer);
2226 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2228 std::string polygonName =
"";
2231 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2232 polygonName = mapOfParams[
"name"];
2234 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2235 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2237 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2241 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2242 minPolygonAreaThreshold);
2244 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2246 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2247 minPolygonAreaThreshold);
2252 std::cerr <<
"Cannot get the number of circles. Defaulting to zero." << std::endl;
2256 startIdFace = idFace;
2259 delete[] caoLinePoints;
2261 if (header && parent) {
2263 std::cout <<
"Global information for " <<
vpIoTools::getName(modelFile) <<
" :" << std::endl;
2264 std::cout <<
"Total nb of points : " <<
nbPoints << std::endl;
2265 std::cout <<
"Total nb of lines : " <<
nbLines << std::endl;
2266 std::cout <<
"Total nb of polygon lines : " <<
nbPolygonLines << std::endl;
2267 std::cout <<
"Total nb of polygon points : " <<
nbPolygonPoints << std::endl;
2268 std::cout <<
"Total nb of cylinders : " <<
nbCylinders << std::endl;
2269 std::cout <<
"Total nb of circles : " <<
nbCircles << std::endl;
2271 std::cout <<
"> " <<
nbPoints <<
" points" << std::endl;
2272 std::cout <<
"> " <<
nbLines <<
" lines" << std::endl;
2273 std::cout <<
"> " <<
nbPolygonLines <<
" polygon lines" << std::endl;
2274 std::cout <<
"> " <<
nbPolygonPoints <<
" polygon points" << std::endl;
2275 std::cout <<
"> " <<
nbCylinders <<
" cylinders" << std::endl;
2276 std::cout <<
"> " <<
nbCircles <<
" circles" << std::endl;
2281 vectorOfModelFilename.pop_back();
2283 std::cerr <<
"Cannot read line!" << std::endl;
2288 #ifdef VISP_HAVE_COIN3D
2299 SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
2300 if (sceneGraphVRML2Trasnform) {
2301 float rx, ry, rz, rw;
2302 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2308 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2309 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2310 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2316 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2317 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2318 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2322 for (
unsigned int i = 0; i < 3; i++)
2324 for (
unsigned int i = 0; i < 3; i++)
2326 for (
unsigned int i = 0; i < 3; i++)
2330 transform = transform * transformCur;
2333 int nbShapes = sceneGraphVRML2->getNumChildren();
2340 for (
int i = 0; i < nbShapes; i++) {
2342 child = sceneGraphVRML2->getChild(i);
2344 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2345 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2348 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2349 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2352 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2353 SoChildList *child2list = child->getChildren();
2354 std::string name = child->getName().getString();
2356 for (
int j = 0; j < child2list->getLength(); j++) {
2357 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2358 SoVRMLIndexedFaceSet *face_set;
2359 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2360 if (!strncmp(face_set->getName().getString(),
"cyl", 3)) {
2366 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2367 SoVRMLIndexedLineSet *line_set;
2368 line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2386 const std::string &polygonName)
2388 std::vector<vpPoint> corners;
2392 int indexListSize = face_set->coordIndex.getNum();
2396 SoVRMLCoordinate *coord;
2398 for (
int i = 0; i < indexListSize; i++) {
2399 if (face_set->coordIndex[i] == -1) {
2400 if (corners.size() > 1) {
2409 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2410 int index = face_set->coordIndex[i];
2411 pointTransformed[0] = coord->point[index].getValue()[0];
2412 pointTransformed[1] = coord->point[index].getValue()[1];
2413 pointTransformed[2] = coord->point[index].getValue()[2];
2414 pointTransformed[3] = 1.0;
2416 pointTransformed = transform * pointTransformed;
2419 corners.push_back(pt);
2439 const std::string &polygonName)
2441 std::vector<vpPoint> corners_c1, corners_c2;
2444 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2446 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
2448 if (indexListSize % 2 == 1) {
2449 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
2452 corners_c1.resize(indexListSize / 2);
2453 corners_c2.resize(indexListSize / 2);
2459 for (
int i = 0; i < coords->point.getNum(); ++i) {
2460 pointTransformed[0] = coords->point[i].getValue()[0];
2461 pointTransformed[1] = coords->point[i].getValue()[1];
2462 pointTransformed[2] = coords->point[i].getValue()[2];
2463 pointTransformed[3] = 1.0;
2465 pointTransformed = transform * pointTransformed;
2469 if (i < (
int)corners_c1.size()) {
2470 corners_c1[(
unsigned int)i] = pt;
2472 corners_c2[(
unsigned int)i - corners_c1.size()] = pt;
2480 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
2481 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
2482 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
2483 double radius_c1 = sqrt(dist.sumSquare());
2484 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
2485 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
2486 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
2487 double radius_c2 = sqrt(dist.sumSquare());
2489 if (std::fabs(radius_c1 - radius_c2) >
2490 (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))) {
2491 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
2498 int idRevolutionAxis = idFace;
2503 std::vector<std::vector<vpPoint> > listFaces;
2507 initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2525 std::vector<vpPoint> corners;
2528 int indexListSize = line_set->coordIndex.getNum();
2530 SbVec3f point(0, 0, 0);
2532 SoVRMLCoordinate *coord;
2534 for (
int i = 0; i < indexListSize; i++) {
2535 if (line_set->coordIndex[i] == -1) {
2536 if (corners.size() > 1) {
2545 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2546 int index = line_set->coordIndex[i];
2547 point[0] = coord->point[index].getValue()[0];
2548 point[1] = coord->point[index].getValue()[1];
2549 point[2] = coord->point[index].getValue()[2];
2552 corners.push_back(pt);
2557 #endif // VISP_HAVE_COIN3D
2571 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
2579 for (
unsigned int i = 0; i < pts.size(); ++i) {
2581 oY += pts[i].get_oY();
2582 oZ += pts[i].get_oZ();
2601 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2605 std::vector<vpPolygon> polygonsTmp;
2606 std::vector<std::vector<vpPoint> > roisPtTmp;
2609 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2614 if ((useVisibility &&
faces.
getPolygon()[i]->isvisible) || !useVisibility) {
2615 std::vector<vpImagePoint> roiPts;
2623 if (roiPts.size() <= 2) {
2627 polygonsTmp.push_back(
vpPolygon(roiPts));
2629 std::vector<vpPoint> polyPts;
2637 roisPtTmp.push_back(polyPts);
2642 if (orderPolygons) {
2644 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2645 for (
unsigned int i = 0; i < polygonsTmp.size(); i++) {
2646 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2647 for (
unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2648 x_centroid += roisPtTmp[i][j].get_X();
2649 y_centroid += roisPtTmp[i][j].get_Y();
2650 z_centroid += roisPtTmp[i][j].get_Z();
2653 x_centroid /= roisPtTmp[i].size();
2654 y_centroid /= roisPtTmp[i].size();
2655 z_centroid /= roisPtTmp[i].size();
2657 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2658 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2662 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2664 polygonsTmp.resize(listOfPolygonFaces.size());
2665 roisPtTmp.resize(listOfPolygonFaces.size());
2668 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2670 polygonsTmp[cpt] = it->polygon;
2671 roisPtTmp[cpt] = it->faceCorners;
2674 pairOfPolygonFaces.first = polygonsTmp;
2675 pairOfPolygonFaces.second = roisPtTmp;
2677 pairOfPolygonFaces.first = polygonsTmp;
2678 pairOfPolygonFaces.second = roisPtTmp;
2681 return pairOfPolygonFaces;
2696 #ifndef VISP_HAVE_OGRE
2698 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2699 "will be used. setOgreVisibilityTest() set to false."
2713 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. "
2714 "Far clipping won't be considered.");
2716 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping "
2717 "won't be considered.");
2721 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2724 #ifdef VISP_HAVE_OGRE
2742 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2743 if (name.empty() ||
faces[i]->name == name) {
2744 faces[i]->setLod(useLod);
2760 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2761 if (name.empty() ||
faces[i]->name == name) {
2762 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2777 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2778 if (name.empty() ||
faces[i]->name == name) {
2779 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2792 vpTRACE(
"Near clipping value cannot be superior than far clipping value. "
2793 "Near clipping won't be considered.");
2795 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping "
2796 "won't be considered.");
2800 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2803 #ifdef VISP_HAVE_OGRE
2819 for (
unsigned int i = 0; i <
faces.
size(); i++)
2833 if (isoJoIdentity_) {
2856 if (interaction.
getRows() != error.getRows() || interaction.
getCols() != 6) {
2869 __m128d v_JTR_0_1 = _mm_setzero_pd();
2870 __m128d v_JTR_2_3 = _mm_setzero_pd();
2871 __m128d v_JTR_4_5 = _mm_setzero_pd();
2873 for (
unsigned int i = 0; i < interaction.
getRows(); i++) {
2874 const __m128d v_error = _mm_set1_pd(error[i]);
2876 __m128d v_interaction = _mm_loadu_pd(&interaction[i][0]);
2877 v_JTR_0_1 = _mm_add_pd(v_JTR_0_1, _mm_mul_pd(v_interaction, v_error));
2879 v_interaction = _mm_loadu_pd(&interaction[i][2]);
2880 v_JTR_2_3 = _mm_add_pd(v_JTR_2_3, _mm_mul_pd(v_interaction, v_error));
2882 v_interaction = _mm_loadu_pd(&interaction[i][4]);
2883 v_JTR_4_5 = _mm_add_pd(v_JTR_4_5, _mm_mul_pd(v_interaction, v_error));
2886 _mm_storeu_pd(JTR.
data, v_JTR_0_1);
2887 _mm_storeu_pd(JTR.
data + 2, v_JTR_2_3);
2888 _mm_storeu_pd(JTR.
data + 4, v_JTR_4_5);
2891 const unsigned int N = interaction.
getRows();
2893 for (
unsigned int i = 0; i < 6; i += 1) {
2895 for (
unsigned int j = 0; j < N; j += 1) {
2896 ssum += interaction[j][i] * error[j];
2905 double &mu,
bool &reStartFromLastIncrement,
vpColVector *
const w,
2909 if (error.sumSquare() / (double)error.getRows() > m_error_prev.
sumSquare() / (double)m_error_prev.
getRows()) {
2916 error = m_error_prev;
2917 if (w != NULL && m_w_prev != NULL) {
2920 reStartFromLastIncrement =
true;
2930 if (isoJoIdentity_) {
2938 vpMatrix LTLmuI = LTL + (LMA * mu);
2945 if (w != NULL && m_w_prev != NULL)
2967 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2975 if (w != NULL && m_w_prev != NULL)
2990 if (error.getRows() > 0)
3008 for (
unsigned int i = 0; i < 6; i++)
3032 for (
unsigned int i = 0; i < 6; i++) {
3034 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3045 std::vector<std::vector<vpPoint> > &listFaces)
3067 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3071 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3075 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon())
3105 std::vector<vpPoint> pointsFace;
3106 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3107 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3108 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3109 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3110 listFaces.push_back(pointsFace);
3113 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3114 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3115 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3116 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3117 listFaces.push_back(pointsFace);
3120 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3121 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3122 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3123 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3124 listFaces.push_back(pointsFace);
3127 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3128 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3129 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3130 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3131 listFaces.push_back(pointsFace);
3149 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3150 dz <= std::numeric_limits<double>::epsilon())
3157 bool useLod,
double minPolygonAreaThreshold,
3158 double minLineLengthThreshold)
3160 std::vector<vpPoint> corners_without_duplicates;
3161 corners_without_duplicates.push_back(corners[0]);
3162 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
3163 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3164 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3165 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3166 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3167 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3168 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3169 corners_without_duplicates.push_back(corners[i + 1]);
3174 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
3182 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3183 polygon.
addPoint(j, corners_without_duplicates[j]);
3199 int idFace,
const std::string &polygonName,
bool useLod,
3200 double minPolygonAreaThreshold)
3227 y[0] = plane.
getA() / norm_Y;
3228 y[1] = plane.
getB() / norm_Y;
3229 y[2] = plane.
getC() / norm_Y;
3232 for (
unsigned int i = 0; i < 3; i++) {
3248 for (
unsigned int i = 0; i < 4; i++) {
3251 w_p = wMc * cMc_90 * c_p;
3274 bool useLod,
double minLineLengthThreshold)
3306 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3309 for (
unsigned int i = 0; i < listFaces.size(); i++) {
3311 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
3312 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
3313 polygon.
addPoint(j, listFaces[i][j]);
3340 bool already_here =
false;
3347 already_here =
true;
3353 if (!already_here) {
3380 const std::string &name)
3382 bool already_here =
false;
3394 if (!already_here) {
3410 const std::string &name)
3412 bool already_here =
false;
3425 if (!already_here) {
3440 double radius,
int idFace,
const std::string &name)
3446 int idFace,
const std::string &name)
3455 for (
unsigned int i = 0; i < nbpt - 1; i++)
3465 for (
unsigned int i = 0; i < nbpt - 1; i++)
3494 unsigned int nbFeatures = 0;
3497 if (nbFeatures > 0) {
3498 return vpMath::deg(totalProjectionError / (
double)nbFeatures);
3530 #ifdef VISP_HAVE_OGRE
3561 double totalProjectionError = 0.0;
3566 for (
size_t a = 0; a < l->
meline.size(); a++) {
3567 if (l->
meline[a] != NULL) {
3568 double lineNormGradient;
3569 unsigned int lineNbFeatures;
3573 totalProjectionError += lineNormGradient;
3574 nbFeatures += lineNbFeatures;
3585 double cylinderNormGradient = 0;
3586 unsigned int cylinderNbFeatures = 0;
3590 totalProjectionError += cylinderNormGradient;
3591 nbFeatures += cylinderNbFeatures;
3595 double cylinderNormGradient = 0;
3596 unsigned int cylinderNbFeatures = 0;
3600 totalProjectionError += cylinderNormGradient;
3601 nbFeatures += cylinderNbFeatures;
3610 double circleNormGradient = 0;
3611 unsigned int circleNbFeatures = 0;
3615 totalProjectionError += circleNormGradient;
3616 nbFeatures += circleNbFeatures;
3620 return totalProjectionError;
3625 bool changed =
false;
3630 #ifdef VISP_HAVE_OGRE
3641 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
3642 if ((*it)->meline[a] != NULL) {
3643 delete (*it)->meline[a];
3644 (*it)->meline[a] = NULL;
3648 (*it)->meline.clear();
3649 (*it)->nbFeature.clear();
3650 (*it)->nbFeatureTotal = 0;
3655 if ((*it)->meline1 != NULL) {
3656 delete (*it)->meline1;
3657 (*it)->meline1 = NULL;
3659 if ((*it)->meline2 != NULL) {
3660 delete (*it)->meline2;
3661 (*it)->meline2 = NULL;
3664 (*it)->nbFeature = 0;
3665 (*it)->nbFeaturel1 = 0;
3666 (*it)->nbFeaturel2 = 0;
3670 if ((*it)->meEllipse != NULL) {
3671 delete (*it)->meEllipse;
3672 (*it)->meEllipse = NULL;
3674 (*it)->nbFeature = 0;
3680 const bool doNotTrack =
true;
3685 bool isvisible =
false;
3689 int index = *itindex;
3709 for (
size_t a = 0; a < l->
meline.size(); a++) {
3710 if (l->
meline[a] != NULL)
3712 if (a < l->nbFeature.size())
3725 bool isvisible =
false;
3759 bool isvisible =
false;
3787 #ifdef VISP_HAVE_PUGIXML
3793 std::cout <<
" *********** Parsing XML for ME projection error ************ " << std::endl;
3794 xmlp.
parse(configFile);
3806 std::cerr <<
"pugixml third-party is not properly built to read config file: " << configFile << std::endl;