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)
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;
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_) {
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,
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)
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());
static double getSobelKernelY(double *filter, unsigned int size)
virtual vpColVector getEstimatedDoF() const
Used to indicate that a value is not in the allowed range.
void setMovingEdge(vpMe *Me)
Implementation of a matrix and operations on matrices.
vpMatrix covarianceMatrix
Covariance matrix.
bool parseBoolean(std::string &input)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Implements a 3D polygon with render functionnalities like clipping.
Class that defines generic functionnalities for display.
virtual void setNbPoint(const unsigned int nb)
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
unsigned int nbLines
Number of lines in CAO model.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
void setCameraParameters(const vpCameraParameters &camera)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
unsigned int getWidth() const
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
std::map< std::string, std::string > parseParameters(std::string &endLine)
unsigned int nbCircles
Number of circles in CAO model.
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
void setNearClippingDistance(const double &dist)
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
std::map< std::string, std::string > mapOfParameterNames
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
double euclideanNorm() const
std::list< int > Lindex_polygon
Index of the faces which contain the line.
unsigned int nbCylinders
Number of cylinders in CAO model.
void setFarClippingDistance(const double &dist)
void setCameraParameters(const vpCameraParameters &camera)
unsigned int nbPoints
Number of points in CAO model.
virtual void loadConfigFile(const std::string &configFile)
Display for windows using GDI (available on any windows 32 platform).
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")=0
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
vpPoint * p1
The first extremity.
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
void setProjectionErrorKernelSize(const unsigned int &size)
void setOgreShowConfigDialog(const bool showConfigDialog)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
double get_oY() const
Get the point Y coordinate in the object frame.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
void setMinPolygonAreaThresh(const double min_polygon_area)
error that can be emited by ViSP classes.
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void set_x(const double x)
Set the point x coordinate in the image plane.
vpPoint * p
corners in the object frame
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Manage the line of a polygon used in the model-based tracker.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int nbFeature
The number of moving edges.
void setName(const std::string &face_name)
Type * data
Address of the first element of the data array.
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
bool useOgre
Use Ogre3d for visibility tests.
unsigned int getCols() const
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
std::string modelFileName
void setIndex(const unsigned int i)
virtual void setEstimatedDoF(const vpColVector &v)
unsigned int getProjectionErrorKernelSize() const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
static const vpColor green
vpPoint * p1
The first extremity on the axe.
static void flush(const vpImage< unsigned char > &I)
void setFarClippingDistance(const double &dist)
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
void projectionErrorVisibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo)
Class that defines what is a point.
vpCameraParameters cam
The camera parameters.
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
static Type maximum(const Type &a, const Type &b)
Implementation of a rotation matrix and operations on such kind of matrices.
virtual void init(const vpImage< unsigned char > &I)=0
bool m_projectionErrorOgreShowConfigDialog
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
Defines a generic 2D polygon.
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)=0
vpColVector & normalize()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
Error that can be emited by the vpTracker class and its derivates.
void setProjectionErrorKernelSize(const unsigned int &size)
Implementation of a polygon of the model used by the model-based tracker.
void setName(const std::string &circle_name)
std::string getName() const
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, const bool verbose=false, const bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpPoint * p2
The second extremity.
void projectionErrorResetMovingEdges()
void diag(const double &val=1.0)
void savePose(const std::string &filename) const
bool useScanLine
Use Scanline for visibility tests.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
void setProjectionErrorMe(const vpMe &me)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
void getProjectionErrorMe(vpMe &me) const
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
VISP_EXPORT bool checkSSE2()
static void display(const vpImage< unsigned char > &I)
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void addPoint(const unsigned int n, const vpPoint &P)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
void set_y(const double y)
Set the point y coordinate in the image plane.
Main methods for a model-based tracker.
std::vector< PolygonType * > & getPolygon()
void setIndex(const unsigned int i)
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, const int idFace=-1, const std::string &polygonName="", const bool useLod=false, const double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
vpMatrix m_SobelX
Sobel kernel in X.
std::string & trim(std::string &s) const
virtual void loadVRMLModel(const std::string &modelFile)
Implementation of a rotation vector as quaternion angle minimal representation.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
double m_lambda
Gain of the virtual visual servoing stage.
void setIsPolygonOriented(const bool &oriented)
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
unsigned int getRows() const
double angleAppears
Angle used to detect a face appearance.
void addPolygon(PolygonType *p)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
std::string poseSavingFilename
void setClipping(const unsigned int &flags)
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
void setMovingEdge(vpMe *Me)
void setVisible(bool _isvisible)
unsigned int size() const
void setName(const std::string &line_name)
vpPoint * p2
The second extremity on the axe.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void setCameraParameters(const vpCameraParameters &camera)
double get_oX() const
Get the point X coordinate in the object frame.
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void setIndex(const unsigned int i)
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace=-1, const std::string &name="")
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
static double deg(double rad)
virtual void setOgreVisibilityTest(const bool &v)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
int getWindowYPosition() const
static void read(vpImage< unsigned char > &I, const std::string &filename)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
bool applyLodSettingInConfig
Implementation of column vector and the associated operations.
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
void setLod(const bool use_lod)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
void setMovingEdge(vpMe *Me)
void setProjectionErrorMovingEdge(const vpMe &me)
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix <L, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
Implementation of a pose vector and operations on poses.
vpMatrix m_SobelY
Sobel kernel in Y.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
bool isVisible(const unsigned int i)
Contains an M-Estimator and various influence function.
error that can be emited by the vpMatrix class and its derivates
double angleDisappears
Angle used to detect a face disappearance.
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, const bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void addPolygon(const int &index)
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace=-1, const std::string &name="")
void setNearClippingDistance(const double &dist)
unsigned int getHeight() const
unsigned int getNbPoint() const
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
static double getSobelKernelX(double *filter, unsigned int size)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
virtual void setClipping(const unsigned int &flags)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double radius
The radius of the cylinder.
void addPolygon(const std::vector< vpPoint > &corners, const int idFace=-1, const std::string &polygonName="", const bool useLod=false, const double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
std::vector< vpMbtMeLine * > meline
The moving edge container.
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
This class defines the container for a plane geometrical structure.
unsigned int clippingFlag
Flags specifying which clipping to used.
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
unsigned int nbFeaturel2
The number of moving edges on line 2.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void addPoint(const vpPoint &P)
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
double distNearClip
Distance for near clipping.
bool useLodGeneral
True if LOD mode is enabled.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
std::vector< unsigned int > nbFeature
The number of moving edges.
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
Class that consider the case of a translation vector.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
vpPoint * p1
The center of the circle.
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(const bool orderPolygons=true, const bool useVisibility=true, const bool clipPolygon=false)
void removeComment(std::ifstream &fileId)
bool useScanLine
Use scanline rendering.
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...
void buildFrom(vpPoint &_p1, vpPoint &_p2)
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")=0
void setMinLineLengthThresh(const double min_line_length)
int getWindowXPosition() const
void resize(const unsigned int i, const bool flagNullify=true)
virtual void setIndex(const int i)
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
void computeFov(const unsigned int &w, const unsigned int &h)