51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpDisplay.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpMatrix.h>
55 #include <visp3/core/vpPoint.h>
56 #include <visp3/vision/vpPose.h>
57 #ifdef VISP_HAVE_MODULE_GUI
58 #include <visp3/gui/vpDisplayGDI.h>
59 #include <visp3/gui/vpDisplayOpenCV.h>
60 #include <visp3/gui/vpDisplayX.h>
62 #include <visp3/core/vpCameraParameters.h>
63 #include <visp3/core/vpColor.h>
64 #include <visp3/core/vpException.h>
65 #include <visp3/core/vpIoTools.h>
66 #include <visp3/core/vpPixelMeterConversion.h>
67 #ifdef VISP_HAVE_MODULE_IO
68 #include <visp3/io/vpImageIo.h>
70 #include <visp3/core/vpCPUFeatures.h>
71 #include <visp3/core/vpIoTools.h>
72 #include <visp3/core/vpMatrixException.h>
73 #include <visp3/core/vpTrackingException.h>
74 #include <visp3/mbt/vpMbTracker.h>
76 #include <visp3/core/vpImageFilter.h>
77 #include <visp3/mbt/vpMbtXmlGenericParser.h>
79 #ifdef VISP_HAVE_COIN3D
81 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
82 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
83 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
84 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
85 #include <Inventor/VRMLnodes/SoVRMLShape.h>
86 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
87 #include <Inventor/actions/SoGetMatrixAction.h>
88 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
89 #include <Inventor/actions/SoSearchAction.h>
90 #include <Inventor/actions/SoToVRML2Action.h>
91 #include <Inventor/actions/SoWriteAction.h>
92 #include <Inventor/misc/SoChildList.h>
93 #include <Inventor/nodes/SoSeparator.h>
96 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
97 #include <emmintrin.h>
98 #define VISP_HAVE_SSE2 1
101 #ifndef DOXYGEN_SHOULD_SKIP_THIS
109 SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
111 std::vector<vpPoint> extremities;
114 double minLineLengthThresh;
121 struct PolygonFaceInfo {
122 PolygonFaceInfo(
const double dist,
const vpPolygon &poly,
const std::vector<vpPoint> &corners)
123 : distanceToCamera(dist), polygon(poly), faceCorners(corners)
127 bool operator<(
const PolygonFaceInfo &pfi)
const {
return distanceToCamera < pfi.distanceToCamera; }
129 double distanceToCamera;
131 std::vector<vpPoint> faceCorners;
134 #endif // DOXYGEN_SHOULD_SKIP_THIS
142 : cam(), cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
143 computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
144 displayFeatures(false), m_optimizationMethod(
vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(
vpMath::rad(89)),
145 angleDisappears(
vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(
vpPolygon3D::NO_CLIPPING),
146 useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
147 nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
148 minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
149 m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
150 m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(),
151 m_projectionErrorFaces(), m_projectionErrorOgreShowConfigDialog(false),
152 m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5,5), m_SobelY(5,5),
153 m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), m_projectionErrorDisplayThickness(1),
154 m_projectionErrorCam(), m_mask(NULL)
159 mapOfParameterNames[
"name"] =
"string";
160 mapOfParameterNames[
"minPolygonAreaThreshold"] =
"number";
161 mapOfParameterNames[
"minLineLengthThreshold"] =
"number";
162 mapOfParameterNames[
"useLod"] =
"boolean";
191 #ifdef VISP_HAVE_MODULE_GUI
231 std::string ext =
".init";
232 std::string str_pose =
"";
233 size_t pos = initFile.rfind(ext);
236 std::fstream finitpos;
238 char s[FILENAME_MAX];
240 if (pos != std::string::npos)
241 str_pose = initFile.substr(0, pos) +
".0.pos";
243 str_pose = initFile +
".0.pos";
245 finitpos.open(str_pose.c_str(), std::ios::in);
246 sprintf(s,
"%s", str_pose.c_str());
251 if (finitpos.fail()) {
252 std::cout <<
"cannot read " << s << std::endl <<
"cMo set to identity" << std::endl;
255 for (
unsigned int i = 0; i < 6; i += 1) {
256 finitpos >> init_pos[i];
262 std::cout <<
"last_cMo : " << std::endl << last_cMo << std::endl;
269 std::cout <<
"No modification : left click " << std::endl;
270 std::cout <<
"Modify initial pose : right click " << std::endl;
296 if (pos != std::string::npos)
297 sprintf(s,
"%s", initFile.c_str());
299 sprintf(s,
"%s.init", initFile.c_str());
301 std::cout <<
"Load 3D points from: " << s << std::endl;
304 std::cout <<
"cannot read " << s << std::endl;
308 #ifdef VISP_HAVE_MODULE_IO
312 const std::string imgExtVec[] = {
".ppm",
".pgm",
".jpg",
".jpeg",
".png"};
314 bool foundHelpImg =
false;
315 if (pos != std::string::npos) {
316 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
317 dispF = initFile.substr(0, pos) + imgExtVec[i];
321 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
322 dispF = initFile + imgExtVec[i];
328 std::cout <<
"Load image to help initialization: " << dispF << std::endl;
329 #if defined VISP_HAVE_X11
331 #elif defined VISP_HAVE_GDI
333 #elif defined VISP_HAVE_OPENCV
339 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
341 "Where to initialize...");
348 if (d_help != NULL) {
353 #else //#ifdef VISP_HAVE_MODULE_IO
355 #endif //#ifdef VISP_HAVE_MODULE_IO
361 finit.ignore(256,
'\n');
362 std::cout <<
"Number of 3D points " << n3d << std::endl;
367 std::vector<vpPoint> P(n3d);
368 for (
unsigned int i = 0; i < n3d; i++) {
376 finit.ignore(256,
'\n');
379 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << pt_3d_tf[0] <<
" " << pt_3d_tf[1] <<
" " << pt_3d_tf[2] << std::endl;
381 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
386 bool isWellInit =
false;
387 while (!isWellInit) {
388 std::vector<vpImagePoint> mem_ip;
389 for (
unsigned int i = 0; i < n3d; i++) {
390 std::ostringstream text;
391 text <<
"Click on point " << i + 1;
394 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
399 std::cout <<
"Click on point " << i + 1 <<
" ";
402 mem_ip.push_back(ip);
408 std::cout <<
"with 2D coordinates: " << ip << std::endl;
417 d1 = d2 = std::numeric_limits<double>::max();
465 if (d_help != NULL) {
471 std::cout <<
"cMo : " << std::endl <<
cMo << std::endl;
487 const std::string &displayFile)
494 std::vector<vpPoint> P;
495 for (
unsigned int i = 0; i < points3D_list.size(); i++)
496 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
498 #ifdef VISP_HAVE_MODULE_IO
503 std::cout <<
"Load image to help initialization: " << displayFile << std::endl;
504 #if defined VISP_HAVE_X11
506 #elif defined VISP_HAVE_GDI
508 #elif defined VISP_HAVE_OPENCV
513 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
515 "Where to initialize...");
520 if (d_help != NULL) {
526 #else //#ifdef VISP_HAVE_MODULE_IO
528 #endif //#ifdef VISP_HAVE_MODULE_IO
531 bool isWellInit =
false;
532 while (!isWellInit) {
533 for (
unsigned int i = 0; i < points3D_list.size(); i++) {
534 std::cout <<
"Click on point " << i + 1 << std::endl;
543 std::cout <<
"Click on point " << ip << std::endl;
552 d1 = d2 = std::numeric_limits<double>::max();
595 if (d_help != NULL) {
602 #endif //#ifdef VISP_HAVE_MODULE_GUI
630 char s[FILENAME_MAX];
633 std::string ext =
".init";
634 size_t pos = initFile.rfind(ext);
636 if (pos == initFile.size() - ext.size() && pos != 0)
637 sprintf(s,
"%s", initFile.c_str());
639 sprintf(s,
"%s.init", initFile.c_str());
641 std::cout <<
"Load 2D/3D points from: " << s << std::endl;
642 finit.open(s, std::ios::in);
644 std::cout <<
"cannot read " << s << std::endl;
654 while (!finit.fail() && (c ==
'#')) {
655 finit.ignore(256,
'\n');
662 finit.ignore(256,
'\n');
663 std::cout <<
"Number of 3D points " << n3d << std::endl;
669 for (
unsigned int i = 0; i < n3d; i++) {
672 while (!finit.fail() && (c ==
'#')) {
673 finit.ignore(256,
'\n');
681 finit.ignore(256,
'\n');
683 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << X <<
" " << Y <<
" " << Z << std::endl;
692 while (!finit.fail() && (c ==
'#')) {
693 finit.ignore(256,
'\n');
700 finit.ignore(256,
'\n');
701 std::cout <<
"Number of 2D points " << n2d << std::endl;
710 "In %s file, number of 2D points %d and number of 3D "
711 "points %d are not equal",
716 for (
unsigned int i = 0; i < n2d; i++) {
719 while (!finit.fail() && (c ==
'#')) {
720 finit.ignore(256,
'\n');
724 double u, v, x = 0, y = 0;
727 finit.ignore(256,
'\n');
730 std::cout <<
"Point " << i + 1 <<
" with 2D coordinates: " << ip << std::endl;
741 d1 = d2 = std::numeric_limits<double>::max();
778 const std::vector<vpPoint> &points3D_list)
780 if (points2D_list.size() != points3D_list.size())
781 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points "
782 "different to the number of 3D points.");
784 size_t size = points3D_list.size();
785 std::vector<vpPoint> P;
788 for (
size_t i = 0; i < size; i++) {
789 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
799 d1 = d2 = std::numeric_limits<double>::max();
844 char s[FILENAME_MAX];
848 std::string ext =
".pos";
849 size_t pos = initFile.rfind(ext);
851 if (pos == initFile.size() - ext.size() && pos != 0)
852 sprintf(s,
"%s", initFile.c_str());
854 sprintf(s,
"%s.pos", initFile.c_str());
856 finit.open(s, std::ios::in);
858 std::cout <<
"cannot read " << s << std::endl;
862 for (
unsigned int i = 0; i < 6; i += 1) {
863 finit >> init_pos[i];
903 std::fstream finitpos;
904 char s[FILENAME_MAX];
906 sprintf(s,
"%s", filename.c_str());
907 finitpos.open(s, std::ios::out);
910 finitpos << init_pos;
914 void vpMbTracker::addPolygon(
const std::vector<vpPoint> &corners,
const int idFace,
const std::string &polygonName,
915 const bool useLod,
const double minPolygonAreaThreshold,
916 const double minLineLengthThreshold)
918 std::vector<vpPoint> corners_without_duplicates;
919 corners_without_duplicates.push_back(corners[0]);
920 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
921 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
922 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
923 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
924 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
925 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
926 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
927 corners_without_duplicates.push_back(corners[i + 1]);
932 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
954 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
955 polygon.
addPoint(j, corners_without_duplicates[j]);
971 const int idFace,
const std::string &polygonName,
const bool useLod,
972 const double minPolygonAreaThreshold)
1005 y[0] = plane.
getA() / norm_Y;
1006 y[1] = plane.
getB() / norm_Y;
1007 y[2] = plane.
getC() / norm_Y;
1010 for (
unsigned int i = 0; i < 3; i++) {
1026 for (
unsigned int i = 0; i < 4; i++) {
1029 w_p = wMc * cMc_90 * c_p;
1052 const bool useLod,
const double minLineLengthThreshold)
1090 const std::string &polygonName,
const bool useLod,
const double minLineLengthThreshold)
1093 for (
unsigned int i = 0; i < listFaces.size(); i++) {
1095 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
1096 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
1097 polygon.
addPoint(j, listFaces[i][j]);
1150 std::string::const_iterator it;
1153 it = modelFile.end();
1154 if ((*(it - 1) ==
'o' && *(it - 2) ==
'a' && *(it - 3) ==
'c' && *(it - 4) ==
'.') ||
1155 (*(it - 1) ==
'O' && *(it - 2) ==
'A' && *(it - 3) ==
'C' && *(it - 4) ==
'.')) {
1156 std::vector<std::string> vectorOfModelFilename;
1164 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose,
true, T);
1165 }
else if ((*(it - 1) ==
'l' && *(it - 2) ==
'r' && *(it - 3) ==
'w' && *(it - 4) ==
'.') ||
1166 (*(it - 1) ==
'L' && *(it - 2) ==
'R' && *(it - 3) ==
'W' && *(it - 4) ==
'.')) {
1211 #ifdef VISP_HAVE_COIN3D
1215 SbBool ok = in.openFile(modelFile.c_str());
1216 SoVRMLGroup *sceneGraphVRML2;
1223 if (!in.isFileVRML2()) {
1224 SoSeparator *sceneGraph = SoDB::readAll(&in);
1225 if (sceneGraph == NULL) {
1229 SoToVRML2Action tovrml2;
1230 tovrml2.apply(sceneGraph);
1232 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1233 sceneGraphVRML2->ref();
1234 sceneGraph->unref();
1236 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1237 if (sceneGraphVRML2 == NULL) {
1239 sceneGraphVRML2->ref();
1248 sceneGraphVRML2->unref();
1250 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1260 while (!fileId.fail() && (c ==
'#')) {
1261 fileId.ignore(256,
'\n');
1264 if (fileId.fail()) {
1272 std::map<std::string, std::string> mapOfParams;
1275 while (!endLine.empty() && !exit) {
1280 endLine =
trim(endLine);
1282 std::string param(it->first +
"=");
1285 if (endLine.compare(0, param.size(), param) == 0) {
1287 endLine = endLine.substr(param.size());
1289 bool parseQuote =
false;
1290 if (it->second ==
"string") {
1292 if (endLine.size() > 2 && endLine[0] ==
'"') {
1294 endLine = endLine.substr(1);
1295 size_t pos = endLine.find_first_of(
'"');
1297 if (pos != std::string::npos) {
1298 mapOfParams[it->first] = endLine.substr(0, pos);
1299 endLine = endLine.substr(pos + 1);
1309 size_t pos1 = endLine.find_first_of(
' ');
1310 size_t pos2 = endLine.find_first_of(
'\t');
1311 size_t pos = pos1 < pos2 ? pos1 : pos2;
1313 mapOfParams[it->first] = endLine.substr(0, pos);
1314 endLine = endLine.substr(pos + 1);
1373 int &startIdFace,
const bool verbose,
const bool parent,
1376 std::ifstream fileId;
1377 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1378 fileId.open(modelFile.c_str(), std::ifstream::in);
1379 if (fileId.fail()) {
1380 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
1385 std::cout <<
"Model file : " << modelFile << std::endl;
1387 vectorOfModelFilename.push_back(modelFile);
1400 fileId >> caoVersion;
1401 fileId.ignore(256,
'\n');
1403 std::cout <<
"in vpMbTracker::loadCAOModel() -> Bad parameter header "
1404 "file : use V0, V1, ...";
1406 "header file : use V0, V1, ...");
1413 std::string prefix =
"load";
1417 bool header =
false;
1418 while (c ==
'l' || c ==
'L') {
1421 getline(fileId, line);
1422 if (!line.compare(0, prefix.size(), prefix)) {
1425 std::string headerPathRead = line.substr(6);
1426 size_t firstIndex = headerPathRead.find_first_of(
"\")");
1427 headerPathRead = headerPathRead.substr(0, firstIndex);
1429 std::string headerPath = headerPathRead;
1441 bool cyclic =
false;
1442 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1443 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1444 if (headerPath == *it) {
1451 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose,
false, T);
1456 std::cout <<
"WARNING Cyclic dependency detected with file " << headerPath <<
" declared in " << modelFile
1469 unsigned int caoNbrPoint;
1470 fileId >> caoNbrPoint;
1471 fileId.ignore(256,
'\n');
1474 if (verbose || vectorOfModelFilename.size() == 1) {
1475 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
1478 if (caoNbrPoint > 100000) {
1482 if (caoNbrPoint == 0 && !header) {
1490 for (
unsigned int k = 0; k < caoNbrPoint; k++) {
1498 if (caoVersion == 2) {
1503 fileId.ignore(256,
'\n');
1513 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1514 unsigned int caoNbrLine;
1515 fileId >> caoNbrLine;
1516 fileId.ignore(256,
'\n');
1519 unsigned int *caoLinePoints = NULL;
1520 if (verbose || vectorOfModelFilename.size() == 1) {
1521 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
1524 if (caoNbrLine > 100000) {
1530 caoLinePoints =
new unsigned int[2 * caoNbrLine];
1532 unsigned int index1, index2;
1535 int idFace = startIdFace;
1537 for (
unsigned int k = 0; k < caoNbrLine; k++) {
1546 fileId.getline(buffer, 256);
1547 std::string endLine(buffer);
1548 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1550 std::string segmentName =
"";
1553 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1554 segmentName = mapOfParams[
"name"];
1556 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1557 minLineLengthThresh = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1559 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1563 SegmentInfo segmentInfo;
1564 segmentInfo.name = segmentName;
1565 segmentInfo.useLod = useLod;
1566 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1568 caoLinePoints[2 * k] = index1;
1569 caoLinePoints[2 * k + 1] = index2;
1571 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1572 std::vector<vpPoint> extremities;
1573 extremities.push_back(caoPoints[index1]);
1574 extremities.push_back(caoPoints[index2]);
1575 segmentInfo.extremities = extremities;
1577 std::pair<unsigned int, unsigned int> key(index1, index2);
1579 segmentTemporaryMap[key] = segmentInfo;
1581 vpTRACE(
" line %d has wrong coordinates.", k);
1592 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1593 unsigned int caoNbrPolygonLine;
1594 fileId >> caoNbrPolygonLine;
1595 fileId.ignore(256,
'\n');
1598 if (verbose || vectorOfModelFilename.size() == 1) {
1599 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon lines" << std::endl;
1602 if (caoNbrPolygonLine > 100000) {
1604 delete[] caoLinePoints;
1609 for (
unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1612 unsigned int nbLinePol;
1613 fileId >> nbLinePol;
1614 std::vector<vpPoint> corners;
1615 if (nbLinePol > 100000) {
1619 for (
unsigned int n = 0; n < nbLinePol; n++) {
1622 if (index >= caoNbrLine) {
1625 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
1626 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
1628 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
1629 faceSegmentKeyVector.push_back(key);
1635 fileId.getline(buffer, 256);
1636 std::string endLine(buffer);
1637 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1639 std::string polygonName =
"";
1642 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1643 polygonName = mapOfParams[
"name"];
1645 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
1646 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
1648 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1660 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
1661 it != segmentTemporaryMap.end(); ++it) {
1662 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
1663 faceSegmentKeyVector.end()) {
1665 it->second.minLineLengthThresh);
1669 it->second.minLineLengthThresh);
1678 unsigned int caoNbrPolygonPoint;
1679 fileId >> caoNbrPolygonPoint;
1680 fileId.ignore(256,
'\n');
1683 if (verbose || vectorOfModelFilename.size() == 1) {
1684 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon points" << std::endl;
1687 if (caoNbrPolygonPoint > 100000) {
1691 for (
unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
1694 unsigned int nbPointPol;
1695 fileId >> nbPointPol;
1696 if (nbPointPol > 100000) {
1699 std::vector<vpPoint> corners;
1700 for (
unsigned int n = 0; n < nbPointPol; n++) {
1702 if (index > caoNbrPoint - 1) {
1705 corners.push_back(caoPoints[index]);
1711 fileId.getline(buffer, 256);
1712 std::string endLine(buffer);
1713 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1715 std::string polygonName =
"";
1718 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1719 polygonName = mapOfParams[
"name"];
1721 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
1722 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
1724 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1736 unsigned int caoNbCylinder;
1743 delete[] caoLinePoints;
1748 fileId >> caoNbCylinder;
1749 fileId.ignore(256,
'\n');
1752 if (verbose || vectorOfModelFilename.size() == 1) {
1753 std::cout <<
"> " << caoNbCylinder <<
" cylinders" << std::endl;
1756 if (caoNbCylinder > 100000) {
1760 for (
unsigned int k = 0; k < caoNbCylinder; ++k) {
1764 unsigned int indexP1, indexP2;
1772 fileId.getline(buffer, 256);
1773 std::string endLine(buffer);
1774 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1776 std::string polygonName =
"";
1779 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1780 polygonName = mapOfParams[
"name"];
1782 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1783 minLineLengthThreshold = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1785 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1789 int idRevolutionAxis = idFace;
1790 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
1792 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
1794 std::vector<std::vector<vpPoint> > listFaces;
1796 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
1798 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
1807 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
1812 unsigned int caoNbCircle;
1819 delete[] caoLinePoints;
1824 fileId >> caoNbCircle;
1825 fileId.ignore(256,
'\n');
1828 if (verbose || vectorOfModelFilename.size() == 1) {
1829 std::cout <<
"> " << caoNbCircle <<
" circles" << std::endl;
1832 if (caoNbCircle > 100000) {
1836 for (
unsigned int k = 0; k < caoNbCircle; ++k) {
1840 unsigned int indexP1, indexP2, indexP3;
1849 fileId.getline(buffer, 256);
1850 std::string endLine(buffer);
1851 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1853 std::string polygonName =
"";
1856 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1857 polygonName = mapOfParams[
"name"];
1859 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
1860 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
1862 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1866 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
1867 minPolygonAreaThreshold);
1869 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
1871 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
1872 minPolygonAreaThreshold);
1877 std::cerr <<
"Cannot get the number of circles. Defaulting to zero." << std::endl;
1881 startIdFace = idFace;
1884 delete[] caoLinePoints;
1886 if (vectorOfModelFilename.size() > 1 && parent) {
1888 std::cout <<
"Global information for " <<
vpIoTools::getName(modelFile) <<
" :" << std::endl;
1889 std::cout <<
"Total nb of points : " <<
nbPoints << std::endl;
1890 std::cout <<
"Total nb of lines : " <<
nbLines << std::endl;
1891 std::cout <<
"Total nb of polygon lines : " <<
nbPolygonLines << std::endl;
1892 std::cout <<
"Total nb of polygon points : " <<
nbPolygonPoints << std::endl;
1893 std::cout <<
"Total nb of cylinders : " <<
nbCylinders << std::endl;
1894 std::cout <<
"Total nb of circles : " <<
nbCircles << std::endl;
1896 std::cout <<
"> " <<
nbPoints <<
" points" << std::endl;
1897 std::cout <<
"> " <<
nbLines <<
" lines" << std::endl;
1898 std::cout <<
"> " <<
nbPolygonLines <<
" polygon lines" << std::endl;
1899 std::cout <<
"> " <<
nbPolygonPoints <<
" polygon points" << std::endl;
1900 std::cout <<
"> " <<
nbCylinders <<
" cylinders" << std::endl;
1901 std::cout <<
"> " <<
nbCircles <<
" circles" << std::endl;
1905 std::cerr <<
"Cannot read line!" << std::endl;
1910 #ifdef VISP_HAVE_COIN3D
1921 SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
1922 if (sceneGraphVRML2Trasnform) {
1923 float rx, ry, rz, rw;
1924 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
1930 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
1931 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
1932 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
1938 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
1939 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
1940 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
1944 for (
unsigned int i = 0; i < 3; i++)
1946 for (
unsigned int i = 0; i < 3; i++)
1948 for (
unsigned int i = 0; i < 3; i++)
1952 transform = transform * transformCur;
1955 int nbShapes = sceneGraphVRML2->getNumChildren();
1962 for (
int i = 0; i < nbShapes; i++) {
1964 child = sceneGraphVRML2->getChild(i);
1966 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
1967 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
1970 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
1971 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
1974 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
1975 SoChildList *child2list = child->getChildren();
1976 std::string name = child->getName().getString();
1978 for (
int j = 0; j < child2list->getLength(); j++) {
1979 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
1980 SoVRMLIndexedFaceSet *face_set;
1981 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
1982 if (!strncmp(face_set->getName().getString(),
"cyl", 3)) {
1988 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
1989 SoVRMLIndexedLineSet *line_set;
1990 line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2008 const std::string &polygonName)
2010 std::vector<vpPoint> corners;
2014 int indexListSize = face_set->coordIndex.getNum();
2018 SoVRMLCoordinate *coord;
2020 for (
int i = 0; i < indexListSize; i++) {
2021 if (face_set->coordIndex[i] == -1) {
2022 if (corners.size() > 1) {
2031 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2032 int index = face_set->coordIndex[i];
2033 pointTransformed[0] = coord->point[index].getValue()[0];
2034 pointTransformed[1] = coord->point[index].getValue()[1];
2035 pointTransformed[2] = coord->point[index].getValue()[2];
2036 pointTransformed[3] = 1.0;
2038 pointTransformed = transform * pointTransformed;
2041 corners.push_back(pt);
2061 const std::string &polygonName)
2063 std::vector<vpPoint> corners_c1, corners_c2;
2066 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2068 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
2070 if (indexListSize % 2 == 1) {
2071 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
2074 corners_c1.resize(indexListSize / 2);
2075 corners_c2.resize(indexListSize / 2);
2081 for (
int i = 0; i < coords->point.getNum(); ++i) {
2082 pointTransformed[0] = coords->point[i].getValue()[0];
2083 pointTransformed[1] = coords->point[i].getValue()[1];
2084 pointTransformed[2] = coords->point[i].getValue()[2];
2085 pointTransformed[3] = 1.0;
2087 pointTransformed = transform * pointTransformed;
2091 if (i < (
int)corners_c1.size()) {
2092 corners_c1[(
unsigned int)i] = pt;
2094 corners_c2[(
unsigned int)i - corners_c1.size()] = pt;
2102 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
2103 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
2104 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
2105 double radius_c1 = sqrt(dist.sumSquare());
2106 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
2107 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
2108 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
2109 double radius_c2 = sqrt(dist.sumSquare());
2111 if (std::fabs(radius_c1 - radius_c2) >
2112 (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))) {
2113 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
2120 int idRevolutionAxis = idFace;
2125 std::vector<std::vector<vpPoint> > listFaces;
2129 initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2147 std::vector<vpPoint> corners;
2150 int indexListSize = line_set->coordIndex.getNum();
2152 SbVec3f point(0, 0, 0);
2154 SoVRMLCoordinate *coord;
2156 for (
int i = 0; i < indexListSize; i++) {
2157 if (line_set->coordIndex[i] == -1) {
2158 if (corners.size() > 1) {
2167 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2168 int index = line_set->coordIndex[i];
2169 point[0] = coord->point[index].getValue()[0];
2170 point[1] = coord->point[index].getValue()[1];
2171 point[2] = coord->point[index].getValue()[2];
2174 corners.push_back(pt);
2179 #endif // VISP_HAVE_COIN3D
2193 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
2201 for (
unsigned int i = 0; i < pts.size(); ++i) {
2203 oY += pts[i].get_oY();
2204 oZ += pts[i].get_oZ();
2223 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2227 std::vector<vpPolygon> polygonsTmp;
2228 std::vector<std::vector<vpPoint> > roisPtTmp;
2231 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2236 if ((useVisibility &&
faces.
getPolygon()[i]->isvisible) || !useVisibility) {
2237 std::vector<vpImagePoint> roiPts;
2245 if (roiPts.size() <= 2) {
2249 polygonsTmp.push_back(
vpPolygon(roiPts));
2251 std::vector<vpPoint> polyPts;
2259 roisPtTmp.push_back(polyPts);
2264 if (orderPolygons) {
2266 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2267 for (
unsigned int i = 0; i < polygonsTmp.size(); i++) {
2268 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2269 for (
unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2270 x_centroid += roisPtTmp[i][j].get_X();
2271 y_centroid += roisPtTmp[i][j].get_Y();
2272 z_centroid += roisPtTmp[i][j].get_Z();
2275 x_centroid /= roisPtTmp[i].size();
2276 y_centroid /= roisPtTmp[i].size();
2277 z_centroid /= roisPtTmp[i].size();
2279 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2280 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2284 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2286 polygonsTmp.resize(listOfPolygonFaces.size());
2287 roisPtTmp.resize(listOfPolygonFaces.size());
2290 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2292 polygonsTmp[cpt] = it->polygon;
2293 roisPtTmp[cpt] = it->faceCorners;
2296 pairOfPolygonFaces.first = polygonsTmp;
2297 pairOfPolygonFaces.second = roisPtTmp;
2299 pairOfPolygonFaces.first = polygonsTmp;
2300 pairOfPolygonFaces.second = roisPtTmp;
2303 return pairOfPolygonFaces;
2318 #ifndef VISP_HAVE_OGRE
2320 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2321 "will be used. setOgreVisibilityTest() set to false."
2335 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. "
2336 "Far clipping won't be considered.");
2338 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping "
2339 "won't be considered.");
2343 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2346 #ifdef VISP_HAVE_OGRE
2364 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2365 if (name.empty() ||
faces[i]->name == name) {
2366 faces[i]->setLod(useLod);
2382 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2383 if (name.empty() ||
faces[i]->name == name) {
2384 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2399 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2400 if (name.empty() ||
faces[i]->name == name) {
2401 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2414 vpTRACE(
"Near clipping value cannot be superior than far clipping value. "
2415 "Near clipping won't be considered.");
2417 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping "
2418 "won't be considered.");
2422 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2425 #ifdef VISP_HAVE_OGRE
2441 for (
unsigned int i = 0; i <
faces.
size(); i++)
2455 if (isoJoIdentity_) {
2478 if (interaction.
getRows() != error.getRows() || interaction.
getCols() != 6) {
2491 __m128d v_JTR_0_1 = _mm_setzero_pd();
2492 __m128d v_JTR_2_3 = _mm_setzero_pd();
2493 __m128d v_JTR_4_5 = _mm_setzero_pd();
2495 for (
unsigned int i = 0; i < interaction.
getRows(); i++) {
2496 const __m128d v_error = _mm_set1_pd(error[i]);
2498 __m128d v_interaction = _mm_loadu_pd(&interaction[i][0]);
2499 v_JTR_0_1 = _mm_add_pd(v_JTR_0_1, _mm_mul_pd(v_interaction, v_error));
2501 v_interaction = _mm_loadu_pd(&interaction[i][2]);
2502 v_JTR_2_3 = _mm_add_pd(v_JTR_2_3, _mm_mul_pd(v_interaction, v_error));
2504 v_interaction = _mm_loadu_pd(&interaction[i][4]);
2505 v_JTR_4_5 = _mm_add_pd(v_JTR_4_5, _mm_mul_pd(v_interaction, v_error));
2508 _mm_storeu_pd(JTR.
data, v_JTR_0_1);
2509 _mm_storeu_pd(JTR.
data + 2, v_JTR_2_3);
2510 _mm_storeu_pd(JTR.
data + 4, v_JTR_4_5);
2513 const unsigned int N = interaction.
getRows();
2515 for (
unsigned int i = 0; i < 6; i += 1) {
2517 for (
unsigned int j = 0; j < N; j += 1) {
2518 ssum += interaction[j][i] * error[j];
2527 double &mu,
bool &reStartFromLastIncrement,
vpColVector *
const w,
2531 if (error.sumSquare() / (double)error.getRows() > m_error_prev.
sumSquare() / (double)m_error_prev.
getRows()) {
2538 error = m_error_prev;
2539 if (w != NULL && m_w_prev != NULL) {
2542 reStartFromLastIncrement =
true;
2552 if (isoJoIdentity_) {
2560 vpMatrix LTLmuI = LTL + (LMA * mu);
2567 if (w != NULL && m_w_prev != NULL)
2589 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2597 if (w != NULL && m_w_prev != NULL)
2612 if (error.getRows() > 0)
2630 for (
unsigned int i = 0; i < 6; i++)
2654 for (
unsigned int i = 0; i < 6; i++) {
2656 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
2667 std::vector<std::vector<vpPoint> > &listFaces)
2689 if (axisOrtho.
euclideanNorm() < std::numeric_limits<double>::epsilon()) {
2693 if (axisOrtho.
euclideanNorm() < std::numeric_limits<double>::epsilon()) {
2697 if (axisOrtho.
euclideanNorm() < std::numeric_limits<double>::epsilon())
2727 std::vector<vpPoint> pointsFace;
2728 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
2729 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
2730 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
2731 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
2732 listFaces.push_back(pointsFace);
2735 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
2736 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
2737 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
2738 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
2739 listFaces.push_back(pointsFace);
2742 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
2743 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
2744 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
2745 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
2746 listFaces.push_back(pointsFace);
2749 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
2750 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
2751 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
2752 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
2753 listFaces.push_back(pointsFace);
2771 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
2772 dz <= std::numeric_limits<double>::epsilon())
2779 const bool useLod,
const double minPolygonAreaThreshold,
2780 const double minLineLengthThreshold)
2782 std::vector<vpPoint> corners_without_duplicates;
2783 corners_without_duplicates.push_back(corners[0]);
2784 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
2785 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
2786 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
2787 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
2788 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
2789 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
2790 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
2791 corners_without_duplicates.push_back(corners[i + 1]);
2796 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
2804 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
2805 polygon.
addPoint(j, corners_without_duplicates[j]);
2821 const int idFace,
const std::string &polygonName,
const bool useLod,
2822 const double minPolygonAreaThreshold)
2849 y[0] = plane.
getA() / norm_Y;
2850 y[1] = plane.
getB() / norm_Y;
2851 y[2] = plane.
getC() / norm_Y;
2854 for (
unsigned int i = 0; i < 3; i++) {
2870 for (
unsigned int i = 0; i < 4; i++) {
2873 w_p = wMc * cMc_90 * c_p;
2896 const bool useLod,
const double minLineLengthThreshold)
2928 const std::string &polygonName,
const bool useLod,
const double minLineLengthThreshold)
2931 for (
unsigned int i = 0; i < listFaces.size(); i++) {
2933 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
2934 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
2935 polygon.
addPoint(j, listFaces[i][j]);
2962 bool already_here =
false;
2969 already_here =
true;
2975 if (!already_here) {
3002 const std::string &name)
3004 bool already_here =
false;
3016 if (!already_here) {
3032 const std::string &name)
3034 bool already_here =
false;
3047 if (!already_here) {
3062 const double radius,
const int idFace,
const std::string &name)
3068 const int idFace,
const std::string &name)
3077 for (
unsigned int i = 0; i < nbpt - 1; i++)
3087 for (
unsigned int i = 0; i < nbpt - 1; i++)
3116 unsigned int nbFeatures = 0;
3119 if (nbFeatures > 0) {
3120 return vpMath::deg(totalProjectionError / (
double)nbFeatures);
3152 #ifdef VISP_HAVE_OGRE
3183 double totalProjectionError = 0.0;
3188 for (
size_t a = 0; a < l->
meline.size(); a++) {
3189 if (l->
meline[a] != NULL) {
3190 double lineNormGradient;
3191 unsigned int lineNbFeatures;
3195 totalProjectionError += lineNormGradient;
3196 nbFeatures += lineNbFeatures;
3207 double cylinderNormGradient = 0;
3208 unsigned int cylinderNbFeatures = 0;
3212 totalProjectionError += cylinderNormGradient;
3213 nbFeatures += cylinderNbFeatures;
3217 double cylinderNormGradient = 0;
3218 unsigned int cylinderNbFeatures = 0;
3222 totalProjectionError += cylinderNormGradient;
3223 nbFeatures += cylinderNbFeatures;
3232 double circleNormGradient = 0;
3233 unsigned int circleNbFeatures = 0;
3237 totalProjectionError += circleNormGradient;
3238 nbFeatures += circleNbFeatures;
3242 return totalProjectionError;
3247 bool changed =
false;
3252 #ifdef VISP_HAVE_OGRE
3263 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
3264 if ((*it)->meline[a] != NULL) {
3265 delete (*it)->meline[a];
3266 (*it)->meline[a] = NULL;
3270 (*it)->meline.clear();
3271 (*it)->nbFeature.clear();
3272 (*it)->nbFeatureTotal = 0;
3277 if ((*it)->meline1 != NULL) {
3278 delete (*it)->meline1;
3279 (*it)->meline1 = NULL;
3281 if ((*it)->meline2 != NULL) {
3282 delete (*it)->meline2;
3283 (*it)->meline2 = NULL;
3286 (*it)->nbFeature = 0;
3287 (*it)->nbFeaturel1 = 0;
3288 (*it)->nbFeaturel2 = 0;
3292 if ((*it)->meEllipse != NULL) {
3293 delete (*it)->meEllipse;
3294 (*it)->meEllipse = NULL;
3296 (*it)->nbFeature = 0;
3302 const bool doNotTrack =
true;
3307 bool isvisible =
false;
3311 int index = *itindex;
3331 for (
size_t a = 0; a < l->
meline.size(); a++) {
3332 if (l->
meline[a] != NULL)
3334 if (a < l->nbFeature.size())
3347 bool isvisible =
false;
3381 bool isvisible =
false;
3409 #ifdef VISP_HAVE_XML2
3415 std::cout <<
" *********** Parsing XML for ME projection error ************ " << std::endl;
3416 xmlp.
parse(configFile);
3428 vpTRACE(
"You need the libXML2 to read the config file %s", configFile.c_str());