46 #include <Simd/SimdLib.hpp>
48 #include <visp3/core/vpColVector.h>
49 #include <visp3/core/vpDisplay.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMatrix.h>
52 #include <visp3/core/vpPoint.h>
53 #include <visp3/vision/vpPose.h>
54 #ifdef VISP_HAVE_MODULE_GUI
55 #include <visp3/gui/vpDisplayGDI.h>
56 #include <visp3/gui/vpDisplayOpenCV.h>
57 #include <visp3/gui/vpDisplayX.h>
59 #include <visp3/core/vpCameraParameters.h>
60 #include <visp3/core/vpColor.h>
61 #include <visp3/core/vpException.h>
62 #include <visp3/core/vpIoTools.h>
63 #include <visp3/core/vpPixelMeterConversion.h>
64 #ifdef VISP_HAVE_MODULE_IO
65 #include <visp3/io/vpImageIo.h>
67 #include <visp3/core/vpCPUFeatures.h>
68 #include <visp3/core/vpIoTools.h>
69 #include <visp3/core/vpMatrixException.h>
70 #include <visp3/core/vpTrackingException.h>
71 #include <visp3/mbt/vpMbTracker.h>
73 #include <visp3/core/vpImageFilter.h>
74 #include <visp3/mbt/vpMbtXmlGenericParser.h>
76 #ifdef VISP_HAVE_COIN3D
78 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
79 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
80 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
81 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
82 #include <Inventor/VRMLnodes/SoVRMLShape.h>
83 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
84 #include <Inventor/actions/SoGetMatrixAction.h>
85 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
86 #include <Inventor/actions/SoSearchAction.h>
87 #include <Inventor/actions/SoToVRML2Action.h>
88 #include <Inventor/actions/SoWriteAction.h>
89 #include <Inventor/misc/SoChildList.h>
90 #include <Inventor/nodes/SoSeparator.h>
93 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
97 #ifndef DOXYGEN_SHOULD_SKIP_THIS
101 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
102 std::mutex g_mutex_cout;
108 SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
110 std::vector<vpPoint> extremities;
113 double minLineLengthThresh;
120 struct PolygonFaceInfo {
121 PolygonFaceInfo(
double dist,
const vpPolygon &poly,
const std::vector<vpPoint> &corners)
122 : distanceToCamera(dist), polygon(poly), faceCorners(corners)
126 bool operator<(
const PolygonFaceInfo &pfi)
const {
return distanceToCamera < pfi.distanceToCamera; }
128 double distanceToCamera;
130 std::vector<vpPoint> faceCorners;
140 std::istream &safeGetline(std::istream &is, std::string &t)
150 std::istream::sentry se(is,
true);
151 std::streambuf *sb = is.rdbuf();
154 int c = sb->sbumpc();
157 }
else if (c ==
'\r') {
158 if (sb->sgetc() ==
'\n')
161 }
else if (c == std::streambuf::traits_type::eof()) {
164 is.setstate(std::ios::eofbit);
180 : m_cam(), m_cMo(), oJo(6, 6), m_isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
181 computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
182 displayFeatures(false), m_optimizationMethod(
vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(
vpMath::rad(89)),
183 angleDisappears(
vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(
vpPolygon3D::NO_CLIPPING),
184 useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
185 nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
186 minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
187 m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
188 m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(),
189 m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5),
190 m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20),
191 m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false),
231 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
239 #ifdef VISP_HAVE_MODULE_GUI
248 std::string ext =
".init";
249 std::string str_pose =
"";
250 size_t pos = initFile.rfind(ext);
253 std::fstream finitpos;
255 std::stringstream ss;
257 if (pos != std::string::npos)
258 str_pose = initFile.substr(0, pos) +
".0.pos";
260 str_pose = initFile +
".0.pos";
262 finitpos.open(str_pose.c_str(), std::ios::in);
268 if (finitpos.fail()) {
269 std::cout <<
"Cannot read " << ss.str() << std::endl <<
"cMo set to identity" << std::endl;
272 for (
unsigned int i = 0; i < 6; i += 1) {
273 finitpos >> init_pos[i];
279 std::cout <<
"Tracker initial pose read from " << ss.str() <<
": " << std::endl << last_cMo << std::endl;
293 std::cout <<
"No modification : left click " << std::endl;
294 std::cout <<
"Modify initial pose : right click " << std::endl;
332 ss.str(std::string());
338 if (pos != std::string::npos) {
345 std::cout <<
"Load 3D points from: " << ss.str() << std::endl;
346 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
347 finit.open(ss.str());
349 finit.open(ss.str().c_str());
352 std::cout <<
"Cannot read " << ss.str() << std::endl;
356 #ifdef VISP_HAVE_MODULE_IO
360 const std::string imgExtVec[] = {
".ppm",
".pgm",
".jpg",
".jpeg",
".png"};
362 bool foundHelpImg =
false;
363 if (pos != std::string::npos) {
364 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
365 dispF = initFile.substr(0, pos) + imgExtVec[i];
369 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
370 dispF = initFile + imgExtVec[i];
376 std::cout <<
"Load image to help initialization: " << dispF << std::endl;
377 #if defined(VISP_HAVE_X11)
379 #elif defined(VISP_HAVE_GDI)
381 #elif defined(HAVE_OPENCV_HIGHGUI)
387 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
391 d_help->init(Iref, winXPos + (
int)width + 80, winYPos,
"Where to initialize...");
398 if (d_help != NULL) {
411 finit.ignore(256,
'\n');
412 std::cout <<
"Number of 3D points " << n3d << std::endl;
418 std::vector<vpPoint> P(n3d);
419 for (
unsigned int i = 0; i < n3d; i++) {
427 finit.ignore(256,
'\n');
430 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << pt_3d_tf[0] <<
" " << pt_3d_tf[1] <<
" "
431 << pt_3d_tf[2] << std::endl;
433 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
438 bool isWellInit =
false;
439 while (!isWellInit) {
440 std::vector<vpImagePoint> mem_ip;
441 for (
unsigned int i = 0; i < n3d; i++) {
442 std::ostringstream text;
443 text <<
"Click on point " << i + 1;
447 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
454 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
460 std::cout <<
"Click on point " << i + 1 <<
" ";
464 mem_ip.push_back(ip);
468 mem_ip.push_back(ip);
475 std::cout <<
"with 2D coordinates: " << ip << std::endl;
537 if (d_help != NULL) {
543 std::cout <<
"cMo : " << std::endl <<
m_cMo << std::endl;
587 initClick(&I, NULL, initFile, displayHelp, T);
624 initClick(NULL, &I_color, initFile, displayHelp, T);
628 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile)
641 std::vector<vpPoint> P;
642 for (
unsigned int i = 0; i < points3D_list.size(); i++)
643 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
645 #ifdef VISP_HAVE_MODULE_IO
650 std::cout <<
"Load image to help initialization: " << displayFile << std::endl;
651 #if defined(VISP_HAVE_X11)
653 #elif defined(VISP_HAVE_GDI)
655 #elif defined VISP_HAVE_OPENCV
660 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
663 "Where to initialize...");
672 if (d_help != NULL) {
683 bool isWellInit =
false;
684 while (!isWellInit) {
685 for (
unsigned int i = 0; i < points3D_list.size(); i++) {
686 std::cout <<
"Click on point " << i + 1 << std::endl;
701 std::cout <<
"Click on point " << ip << std::endl;
762 if (d_help != NULL) {
787 const std::string &displayFile)
789 initClick(&I, NULL, points3D_list, displayFile);
804 const std::string &displayFile)
806 initClick(NULL, &I_color, points3D_list, displayFile);
811 const std::string &initFile)
813 std::stringstream ss;
816 std::string ext =
".init";
817 size_t pos = initFile.rfind(ext);
819 if (pos == initFile.size() - ext.size() && pos != 0) {
826 std::cout <<
"Load 2D/3D points from: " << ss.str() << std::endl;
827 finit.open(ss.str().c_str(), std::ios::in);
829 std::cout <<
"cannot read " << ss.str() << std::endl;
839 while (!finit.fail() && (c ==
'#')) {
840 finit.ignore(256,
'\n');
847 finit.ignore(256,
'\n');
848 std::cout <<
"Number of 3D points " << n3d << std::endl;
855 for (
unsigned int i = 0; i < n3d; i++) {
858 while (!finit.fail() && (c ==
'#')) {
859 finit.ignore(256,
'\n');
867 finit.ignore(256,
'\n');
869 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << X <<
" " << Y <<
" " << Z << std::endl;
878 while (!finit.fail() && (c ==
'#')) {
879 finit.ignore(256,
'\n');
886 finit.ignore(256,
'\n');
887 std::cout <<
"Number of 2D points " << n2d << std::endl;
897 "In %s file, number of 2D points %d and number of 3D "
898 "points %d are not equal",
899 ss.str().c_str(), n2d, n3d);
903 for (
unsigned int i = 0; i < n2d; i++) {
906 while (!finit.fail() && (c ==
'#')) {
907 finit.ignore(256,
'\n');
911 double u, v, x = 0, y = 0;
914 finit.ignore(256,
'\n');
917 std::cout <<
"Point " << i + 1 <<
" with 2D coordinates: " << ip << std::endl;
997 const std::vector<vpImagePoint> &points2D_list,
998 const std::vector<vpPoint> &points3D_list)
1000 if (points2D_list.size() != points3D_list.size())
1001 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points "
1002 "different to the number of 3D points.");
1004 size_t size = points3D_list.size();
1005 std::vector<vpPoint> P;
1008 for (
size_t i = 0; i < size; i++) {
1009 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1010 double x = 0, y = 0;
1036 const std::vector<vpPoint> &points3D_list)
1050 const std::vector<vpPoint> &points3D_list)
1056 const std::string &initFile)
1058 std::stringstream ss;
1062 std::string ext =
".pos";
1063 size_t pos = initFile.rfind(ext);
1065 if (pos == initFile.size() - ext.size() && pos != 0) {
1072 finit.open(ss.str().c_str(), std::ios::in);
1074 std::cout <<
"Cannot read " << ss.str() << std::endl;
1078 for (
unsigned int i = 0; i < 6; i += 1) {
1079 finit >> init_pos[i];
1196 std::fstream finitpos;
1197 finitpos.open(filename.c_str(), std::ios::out);
1200 finitpos << init_pos;
1205 bool useLod,
double minPolygonAreaThreshold,
double minLineLengthThreshold)
1207 std::vector<vpPoint> corners_without_duplicates;
1208 corners_without_duplicates.push_back(corners[0]);
1209 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
1210 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1211 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1212 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1213 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1214 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1215 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1216 corners_without_duplicates.push_back(corners[i + 1]);
1221 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
1243 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1244 polygon.
addPoint(j, corners_without_duplicates[j]);
1260 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold)
1293 y[0] = plane.
getA() / norm_Y;
1294 y[1] = plane.
getB() / norm_Y;
1295 y[2] = plane.
getC() / norm_Y;
1298 for (
unsigned int i = 0; i < 3; i++) {
1314 for (
unsigned int i = 0; i < 4; i++) {
1317 w_p = wMc * cMc_90 * c_p;
1340 bool useLod,
double minLineLengthThreshold)
1378 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
1381 for (
unsigned int i = 0; i < listFaces.size(); i++) {
1383 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
1384 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
1385 polygon.
addPoint(j, listFaces[i][j]);
1426 std::string::const_iterator it;
1429 it = modelFile.end();
1430 if ((*(it - 1) ==
'o' && *(it - 2) ==
'a' && *(it - 3) ==
'c' && *(it - 4) ==
'.') ||
1431 (*(it - 1) ==
'O' && *(it - 2) ==
'A' && *(it - 3) ==
'C' && *(it - 4) ==
'.')) {
1432 std::vector<std::string> vectorOfModelFilename;
1440 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose,
true, odTo);
1441 }
else if ((*(it - 1) ==
'l' && *(it - 2) ==
'r' && *(it - 3) ==
'w' && *(it - 4) ==
'.') ||
1442 (*(it - 1) ==
'L' && *(it - 2) ==
'R' && *(it - 3) ==
'W' && *(it - 4) ==
'.')) {
1475 #ifdef VISP_HAVE_COIN3D
1480 SbBool ok = in.openFile(modelFile.c_str());
1481 SoVRMLGroup *sceneGraphVRML2;
1488 if (!in.isFileVRML2()) {
1489 SoSeparator *sceneGraph = SoDB::readAll(&in);
1490 if (sceneGraph == NULL) {
1494 SoToVRML2Action tovrml2;
1495 tovrml2.apply(sceneGraph);
1497 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1498 sceneGraphVRML2->ref();
1499 sceneGraph->unref();
1501 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1502 if (sceneGraphVRML2 == NULL) {
1504 sceneGraphVRML2->ref();
1513 sceneGraphVRML2->unref();
1515 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1525 while (!fileId.fail() && (c ==
'#')) {
1526 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1529 if (fileId.fail()) {
1537 std::map<std::string, std::string> mapOfParams;
1540 while (!endLine.empty() && !exit) {
1546 std::string param(it->first +
"=");
1549 if (endLine.compare(0, param.size(), param) == 0) {
1551 endLine = endLine.substr(param.size());
1553 bool parseQuote =
false;
1554 if (it->second ==
"string") {
1556 if (endLine.size() > 2 && endLine[0] ==
'"') {
1558 endLine = endLine.substr(1);
1559 size_t pos = endLine.find_first_of(
'"');
1561 if (pos != std::string::npos) {
1562 mapOfParams[it->first] = endLine.substr(0, pos);
1563 endLine = endLine.substr(pos + 1);
1573 size_t pos1 = endLine.find_first_of(
' ');
1574 size_t pos2 = endLine.find_first_of(
'\t');
1575 size_t pos = pos1 < pos2 ? pos1 : pos2;
1577 mapOfParams[it->first] = endLine.substr(0, pos);
1578 endLine = endLine.substr(pos + 1);
1639 std::ifstream fileId;
1640 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1641 fileId.open(modelFile.c_str(), std::ifstream::in);
1642 if (fileId.fail()) {
1643 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
1648 std::cout <<
"Model file : " << modelFile << std::endl;
1650 vectorOfModelFilename.push_back(modelFile);
1663 fileId >> caoVersion;
1664 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1666 std::cout <<
"in vpMbTracker::loadCAOModel() -> Bad parameter header "
1667 "file : use V0, V1, ...";
1669 "header file : use V0, V1, ...");
1676 const std::string prefix_load =
"load";
1680 bool header =
false;
1681 while (c ==
'l' || c ==
'L') {
1682 getline(fileId, line);
1684 if (!line.compare(0, prefix_load.size(), prefix_load)) {
1686 std::string paramsStr = line.substr(5);
1688 paramsStr = paramsStr.substr(0, paramsStr.find_first_of(
")"));
1692 for (
size_t i = 0; i < params.size(); i++) {
1696 if (!params.empty()) {
1698 std::string headerPathRead = params[0];
1699 headerPathRead = headerPathRead.substr(1);
1700 headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of(
"\""));
1702 std::string headerPath = headerPathRead;
1717 for (
size_t i = 1; i < params.size(); i++) {
1718 std::string param = params[i];
1720 const std::string prefix =
"t=[";
1721 if (!param.compare(0, prefix.size(), prefix)) {
1722 param = param.substr(prefix.size());
1723 param = param.substr(0, param.find_first_of(
"]"));
1726 if (values.size() == 3) {
1727 t[0] = atof(values[0].c_str());
1728 t[1] = atof(values[1].c_str());
1729 t[2] = atof(values[2].c_str());
1734 const std::string prefix =
"tu=[";
1735 if (!param.compare(0, prefix.size(), prefix)) {
1736 param = param.substr(prefix.size());
1737 param = param.substr(0, param.find_first_of(
"]"));
1740 if (values.size() == 3) {
1741 for (
size_t j = 0; j < values.size(); j++) {
1742 std::string value = values[j];
1744 size_t unitPos = value.find(
"deg");
1745 if (unitPos != std::string::npos) {
1746 value = value.substr(0, unitPos);
1750 unitPos = value.find(
"rad");
1751 if (unitPos != std::string::npos) {
1752 value = value.substr(0, unitPos);
1754 tu[
static_cast<unsigned int>(j)] = !radian ?
vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1762 bool cyclic =
false;
1763 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1764 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1765 if (headerPath == *it) {
1773 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose,
false, odTo * oTo_local);
1778 std::cout <<
"WARNING Cyclic dependency detected with file " << headerPath <<
" declared in " << modelFile
1790 unsigned int caoNbrPoint;
1791 fileId >> caoNbrPoint;
1792 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1795 if (verbose || (parent && !header)) {
1796 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1797 std::lock_guard<std::mutex> lock(g_mutex_cout);
1799 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
1802 if (caoNbrPoint > 100000) {
1806 if (caoNbrPoint == 0 && !header) {
1814 for (
unsigned int k = 0; k < caoNbrPoint; k++) {
1822 if (caoVersion == 2) {
1827 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1837 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1838 unsigned int caoNbrLine;
1839 fileId >> caoNbrLine;
1840 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1843 unsigned int *caoLinePoints = NULL;
1844 if (verbose || (parent && !header)) {
1845 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1846 std::lock_guard<std::mutex> lock(g_mutex_cout);
1848 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
1851 if (caoNbrLine > 100000) {
1857 caoLinePoints =
new unsigned int[2 * caoNbrLine];
1859 unsigned int index1, index2;
1862 int idFace = startIdFace;
1864 for (
unsigned int k = 0; k < caoNbrLine; k++) {
1872 std::string endLine =
"";
1873 if (safeGetline(fileId, endLine).good()) {
1874 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1876 std::string segmentName =
"";
1879 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1880 segmentName = mapOfParams[
"name"];
1882 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1883 minLineLengthThresh = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1885 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1889 SegmentInfo segmentInfo;
1890 segmentInfo.name = segmentName;
1891 segmentInfo.useLod = useLod;
1892 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1894 caoLinePoints[2 * k] = index1;
1895 caoLinePoints[2 * k + 1] = index2;
1897 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1898 std::vector<vpPoint> extremities;
1899 extremities.push_back(caoPoints[index1]);
1900 extremities.push_back(caoPoints[index2]);
1901 segmentInfo.extremities = extremities;
1903 std::pair<unsigned int, unsigned int> key(index1, index2);
1905 segmentTemporaryMap[key] = segmentInfo;
1907 vpTRACE(
" line %d has wrong coordinates.", k);
1919 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1920 unsigned int caoNbrPolygonLine;
1921 fileId >> caoNbrPolygonLine;
1922 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1925 if (verbose || (parent && !header)) {
1926 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1927 std::lock_guard<std::mutex> lock(g_mutex_cout);
1929 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon lines" << std::endl;
1932 if (caoNbrPolygonLine > 100000) {
1934 delete[] caoLinePoints;
1939 for (
unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1942 unsigned int nbLinePol;
1943 fileId >> nbLinePol;
1944 std::vector<vpPoint> corners;
1945 if (nbLinePol > 100000) {
1949 for (
unsigned int n = 0; n < nbLinePol; n++) {
1952 if (index >= caoNbrLine) {
1955 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
1956 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
1958 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
1959 faceSegmentKeyVector.push_back(key);
1964 std::string endLine =
"";
1965 if (safeGetline(fileId, endLine).good()) {
1966 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1968 std::string polygonName =
"";
1971 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1972 polygonName = mapOfParams[
"name"];
1974 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
1975 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
1977 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1991 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
1992 it != segmentTemporaryMap.end(); ++it) {
1993 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
1994 faceSegmentKeyVector.end()) {
1996 it->second.minLineLengthThresh);
2009 unsigned int caoNbrPolygonPoint;
2010 fileId >> caoNbrPolygonPoint;
2011 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2014 if (verbose || (parent && !header)) {
2015 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2016 std::lock_guard<std::mutex> lock(g_mutex_cout);
2018 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon points" << std::endl;
2021 if (caoNbrPolygonPoint > 100000) {
2025 for (
unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2028 unsigned int nbPointPol;
2029 fileId >> nbPointPol;
2030 if (nbPointPol > 100000) {
2033 std::vector<vpPoint> corners;
2034 for (
unsigned int n = 0; n < nbPointPol; n++) {
2036 if (index > caoNbrPoint - 1) {
2039 corners.push_back(caoPoints[index]);
2044 std::string endLine =
"";
2045 if (safeGetline(fileId, endLine).good()) {
2046 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2048 std::string polygonName =
"";
2051 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2052 polygonName = mapOfParams[
"name"];
2054 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2055 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2057 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2071 unsigned int caoNbCylinder;
2078 delete[] caoLinePoints;
2083 fileId >> caoNbCylinder;
2084 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2087 if (verbose || (parent && !header)) {
2088 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2089 std::lock_guard<std::mutex> lock(g_mutex_cout);
2091 std::cout <<
"> " << caoNbCylinder <<
" cylinders" << std::endl;
2094 if (caoNbCylinder > 100000) {
2098 for (
unsigned int k = 0; k < caoNbCylinder; ++k) {
2102 unsigned int indexP1, indexP2;
2109 std::string endLine =
"";
2110 if (safeGetline(fileId, endLine).good()) {
2111 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2113 std::string polygonName =
"";
2116 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2117 polygonName = mapOfParams[
"name"];
2119 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
2120 minLineLengthThreshold = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
2122 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2126 int idRevolutionAxis = idFace;
2127 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2130 minLineLengthThreshold);
2132 std::vector<std::vector<vpPoint> > listFaces;
2134 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2136 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2145 }
catch (
const std::exception &e) {
2146 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2147 std::cerr <<
"Exception: " << e.what() << std::endl;
2152 unsigned int caoNbCircle;
2159 delete[] caoLinePoints;
2164 fileId >> caoNbCircle;
2165 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2168 if (verbose || (parent && !header)) {
2169 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2170 std::lock_guard<std::mutex> lock(g_mutex_cout);
2172 std::cout <<
"> " << caoNbCircle <<
" circles" << std::endl;
2175 if (caoNbCircle > 100000) {
2179 for (
unsigned int k = 0; k < caoNbCircle; ++k) {
2183 unsigned int indexP1, indexP2, indexP3;
2191 std::string endLine =
"";
2192 if (safeGetline(fileId, endLine).good()) {
2193 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2195 std::string polygonName =
"";
2198 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2199 polygonName = mapOfParams[
"name"];
2201 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2202 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2204 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2208 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2209 minPolygonAreaThreshold);
2211 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2214 polygonName, useLod, minPolygonAreaThreshold);
2220 }
catch (
const std::exception &e) {
2221 std::cerr <<
"Cannot get the number of circles. Defaulting to zero." << std::endl;
2222 std::cerr <<
"Exception: " << e.what() << std::endl;
2226 startIdFace = idFace;
2229 delete[] caoLinePoints;
2231 if (header && parent) {
2233 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2234 std::lock_guard<std::mutex> lock(g_mutex_cout);
2236 std::cout <<
"Global information for " <<
vpIoTools::getName(modelFile) <<
" :" << std::endl;
2237 std::cout <<
"Total nb of points : " <<
nbPoints << std::endl;
2238 std::cout <<
"Total nb of lines : " <<
nbLines << std::endl;
2239 std::cout <<
"Total nb of polygon lines : " <<
nbPolygonLines << std::endl;
2240 std::cout <<
"Total nb of polygon points : " <<
nbPolygonPoints << std::endl;
2241 std::cout <<
"Total nb of cylinders : " <<
nbCylinders << std::endl;
2242 std::cout <<
"Total nb of circles : " <<
nbCircles << std::endl;
2244 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
2245 std::lock_guard<std::mutex> lock(g_mutex_cout);
2247 std::cout <<
"> " <<
nbPoints <<
" points" << std::endl;
2248 std::cout <<
"> " <<
nbLines <<
" lines" << std::endl;
2249 std::cout <<
"> " <<
nbPolygonLines <<
" polygon lines" << std::endl;
2250 std::cout <<
"> " <<
nbPolygonPoints <<
" polygon points" << std::endl;
2251 std::cout <<
"> " <<
nbCylinders <<
" cylinders" << std::endl;
2252 std::cout <<
"> " <<
nbCircles <<
" circles" << std::endl;
2257 vectorOfModelFilename.pop_back();
2258 }
catch (
const std::exception &e) {
2259 std::cerr <<
"Cannot read line!" << std::endl;
2260 std::cerr <<
"Exception: " << e.what() << std::endl;
2265 #ifdef VISP_HAVE_COIN3D
2276 SoVRMLTransform *sceneGraphVRML2Trasnform =
dynamic_cast<SoVRMLTransform *
>(sceneGraphVRML2);
2277 if (sceneGraphVRML2Trasnform) {
2278 float rx, ry, rz, rw;
2279 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2285 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2286 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2287 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2293 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2294 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2295 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2299 for (
unsigned int i = 0; i < 3; i++)
2301 for (
unsigned int i = 0; i < 3; i++)
2303 for (
unsigned int i = 0; i < 3; i++)
2307 transform = transform * transformCur;
2310 int nbShapes = sceneGraphVRML2->getNumChildren();
2317 for (
int i = 0; i < nbShapes; i++) {
2319 child = sceneGraphVRML2->getChild(i);
2321 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2322 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2325 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2326 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2329 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2330 SoChildList *child2list = child->getChildren();
2331 std::string name = child->getName().getString();
2333 for (
int j = 0; j < child2list->getLength(); j++) {
2334 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2335 SoVRMLIndexedFaceSet *face_set;
2336 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2337 if (!strncmp(face_set->getName().getString(),
"cyl", 3)) {
2343 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2344 SoVRMLIndexedLineSet *line_set;
2345 line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2363 const std::string &polygonName)
2365 std::vector<vpPoint> corners;
2369 int indexListSize = face_set->coordIndex.getNum();
2373 SoVRMLCoordinate *coord;
2375 for (
int i = 0; i < indexListSize; i++) {
2376 if (face_set->coordIndex[i] == -1) {
2377 if (corners.size() > 1) {
2386 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2387 int index = face_set->coordIndex[i];
2388 pointTransformed[0] = coord->point[index].getValue()[0];
2389 pointTransformed[1] = coord->point[index].getValue()[1];
2390 pointTransformed[2] = coord->point[index].getValue()[2];
2391 pointTransformed[3] = 1.0;
2393 pointTransformed = transform * pointTransformed;
2396 corners.push_back(pt);
2416 const std::string &polygonName)
2418 std::vector<vpPoint> corners_c1, corners_c2;
2421 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2423 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
2425 if (indexListSize % 2 == 1) {
2426 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
2429 corners_c1.resize(indexListSize / 2);
2430 corners_c2.resize(indexListSize / 2);
2436 for (
int i = 0; i < coords->point.getNum(); ++i) {
2437 pointTransformed[0] = coords->point[i].getValue()[0];
2438 pointTransformed[1] = coords->point[i].getValue()[1];
2439 pointTransformed[2] = coords->point[i].getValue()[2];
2440 pointTransformed[3] = 1.0;
2442 pointTransformed = transform * pointTransformed;
2446 if (i < (
int)corners_c1.size()) {
2447 corners_c1[(
unsigned int)i] = pt;
2449 corners_c2[(
unsigned int)i - corners_c1.size()] = pt;
2457 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
2458 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
2459 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
2460 double radius_c1 = sqrt(dist.
sumSquare());
2461 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
2462 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
2463 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
2464 double radius_c2 = sqrt(dist.
sumSquare());
2466 if (std::fabs(radius_c1 - radius_c2) >
2467 (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))) {
2468 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
2475 int idRevolutionAxis = idFace;
2480 std::vector<std::vector<vpPoint> > listFaces;
2484 initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2502 std::vector<vpPoint> corners;
2505 int indexListSize = line_set->coordIndex.getNum();
2507 SbVec3f point(0, 0, 0);
2509 SoVRMLCoordinate *coord;
2511 for (
int i = 0; i < indexListSize; i++) {
2512 if (line_set->coordIndex[i] == -1) {
2513 if (corners.size() > 1) {
2522 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2523 int index = line_set->coordIndex[i];
2524 point[0] = coord->point[index].getValue()[0];
2525 point[1] = coord->point[index].getValue()[1];
2526 point[2] = coord->point[index].getValue()[2];
2529 corners.push_back(pt);
2548 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
2556 for (
unsigned int i = 0; i < pts.size(); ++i) {
2558 oY += pts[i].get_oY();
2559 oZ += pts[i].get_oZ();
2578 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2582 std::vector<vpPolygon> polygonsTmp;
2583 std::vector<std::vector<vpPoint> > roisPtTmp;
2586 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2591 if ((useVisibility &&
faces.
getPolygon()[i]->isvisible) || !useVisibility) {
2592 std::vector<vpImagePoint> roiPts;
2600 if (roiPts.size() <= 2) {
2604 polygonsTmp.push_back(
vpPolygon(roiPts));
2606 std::vector<vpPoint> polyPts;
2614 roisPtTmp.push_back(polyPts);
2619 if (orderPolygons) {
2621 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2622 for (
unsigned int i = 0; i < polygonsTmp.size(); i++) {
2623 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2624 for (
unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2625 x_centroid += roisPtTmp[i][j].get_X();
2626 y_centroid += roisPtTmp[i][j].get_Y();
2627 z_centroid += roisPtTmp[i][j].get_Z();
2630 x_centroid /= roisPtTmp[i].size();
2631 y_centroid /= roisPtTmp[i].size();
2632 z_centroid /= roisPtTmp[i].size();
2634 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2635 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2639 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2641 polygonsTmp.resize(listOfPolygonFaces.size());
2642 roisPtTmp.resize(listOfPolygonFaces.size());
2645 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2647 polygonsTmp[cpt] = it->polygon;
2648 roisPtTmp[cpt] = it->faceCorners;
2651 pairOfPolygonFaces.first = polygonsTmp;
2652 pairOfPolygonFaces.second = roisPtTmp;
2654 pairOfPolygonFaces.first = polygonsTmp;
2655 pairOfPolygonFaces.second = roisPtTmp;
2658 return pairOfPolygonFaces;
2673 #ifndef VISP_HAVE_OGRE
2675 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2676 "will be used. setOgreVisibilityTest() set to false."
2690 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. "
2691 "Far clipping won't be considered.");
2693 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping "
2694 "won't be considered.");
2698 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2701 #ifdef VISP_HAVE_OGRE
2719 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2720 if (name.empty() ||
faces[i]->name == name) {
2721 faces[i]->setLod(useLod);
2737 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2738 if (name.empty() ||
faces[i]->name == name) {
2739 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2754 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2755 if (name.empty() ||
faces[i]->name == name) {
2756 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2769 vpTRACE(
"Near clipping value cannot be superior than far clipping value. "
2770 "Near clipping won't be considered.");
2772 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping "
2773 "won't be considered.");
2777 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2780 #ifdef VISP_HAVE_OGRE
2796 for (
unsigned int i = 0; i <
faces.
size(); i++)
2810 if (isoJoIdentity) {
2843 double &mu,
bool &reStartFromLastIncrement,
vpColVector *
const w,
2854 error = m_error_prev;
2855 if (w != NULL && m_w_prev != NULL) {
2858 reStartFromLastIncrement =
true;
2868 if (isoJoIdentity) {
2876 vpMatrix LTLmuI = LTL + (LMA * mu);
2883 if (w != NULL && m_w_prev != NULL)
2905 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2913 if (w != NULL && m_w_prev != NULL)
2946 for (
unsigned int i = 0; i < 6; i++)
2970 for (
unsigned int i = 0; i < 6; i++) {
2972 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
2983 std::vector<std::vector<vpPoint> > &listFaces)
3005 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3009 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3013 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon())
3043 std::vector<vpPoint> pointsFace;
3044 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3045 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3046 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3047 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3048 listFaces.push_back(pointsFace);
3051 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3052 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3053 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3054 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3055 listFaces.push_back(pointsFace);
3058 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3059 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3060 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3061 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3062 listFaces.push_back(pointsFace);
3065 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3066 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3067 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3068 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3069 listFaces.push_back(pointsFace);
3087 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3088 dz <= std::numeric_limits<double>::epsilon())
3095 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold,
3096 double minLineLengthThreshold)
3098 std::vector<vpPoint> corners_without_duplicates;
3099 corners_without_duplicates.push_back(corners[0]);
3100 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
3101 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3102 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3103 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3104 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3105 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3106 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3107 corners_without_duplicates.push_back(corners[i + 1]);
3112 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
3120 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3121 polygon.
addPoint(j, corners_without_duplicates[j]);
3137 int idFace,
const std::string &polygonName,
bool useLod,
3138 double minPolygonAreaThreshold)
3165 y[0] = plane.
getA() / norm_Y;
3166 y[1] = plane.
getB() / norm_Y;
3167 y[2] = plane.
getC() / norm_Y;
3170 for (
unsigned int i = 0; i < 3; i++) {
3186 for (
unsigned int i = 0; i < 4; i++) {
3189 w_p = wMc * cMc_90 * c_p;
3212 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3244 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3247 for (
unsigned int i = 0; i < listFaces.size(); i++) {
3249 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
3250 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
3251 polygon.
addPoint(j, listFaces[i][j]);
3278 bool already_here =
false;
3285 already_here =
true;
3291 if (!already_here) {
3318 int idFace,
const std::string &name)
3320 bool already_here =
false;
3333 if (!already_here) {
3349 const std::string &name)
3351 bool already_here =
false;
3364 if (!already_here) {
3379 int idFace,
const std::string &name)
3385 const std::string &name)
3394 for (
unsigned int i = 0; i < nbpt - 1; i++)
3404 for (
unsigned int i = 0; i < nbpt - 1; i++)
3433 unsigned int nbFeatures = 0;
3436 if (nbFeatures > 0) {
3437 return vpMath::deg(totalProjectionError / (
double)nbFeatures);
3469 #ifdef VISP_HAVE_OGRE
3500 double totalProjectionError = 0.0;
3505 for (
size_t a = 0; a < l->
meline.size(); a++) {
3506 if (l->
meline[a] != NULL) {
3507 double lineNormGradient;
3508 unsigned int lineNbFeatures;
3512 totalProjectionError += lineNormGradient;
3513 nbFeatures += lineNbFeatures;
3524 double cylinderNormGradient = 0;
3525 unsigned int cylinderNbFeatures = 0;
3529 totalProjectionError += cylinderNormGradient;
3530 nbFeatures += cylinderNbFeatures;
3534 double cylinderNormGradient = 0;
3535 unsigned int cylinderNbFeatures = 0;
3539 totalProjectionError += cylinderNormGradient;
3540 nbFeatures += cylinderNbFeatures;
3549 double circleNormGradient = 0;
3550 unsigned int circleNbFeatures = 0;
3554 totalProjectionError += circleNormGradient;
3555 nbFeatures += circleNbFeatures;
3559 return totalProjectionError;
3564 bool changed =
false;
3570 #ifdef VISP_HAVE_OGRE
3584 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
3585 if ((*it)->meline[a] != NULL) {
3586 delete (*it)->meline[a];
3587 (*it)->meline[a] = NULL;
3591 (*it)->meline.clear();
3592 (*it)->nbFeature.clear();
3593 (*it)->nbFeatureTotal = 0;
3598 if ((*it)->meline1 != NULL) {
3599 delete (*it)->meline1;
3600 (*it)->meline1 = NULL;
3602 if ((*it)->meline2 != NULL) {
3603 delete (*it)->meline2;
3604 (*it)->meline2 = NULL;
3607 (*it)->nbFeature = 0;
3608 (*it)->nbFeaturel1 = 0;
3609 (*it)->nbFeaturel2 = 0;
3614 if ((*it)->meEllipse != NULL) {
3615 delete (*it)->meEllipse;
3616 (*it)->meEllipse = NULL;
3618 (*it)->nbFeature = 0;
3624 const bool doNotTrack =
true;
3629 bool isvisible =
false;
3633 int index = *itindex;
3653 for (
size_t a = 0; a < l->
meline.size(); a++) {
3654 if (l->
meline[a] != NULL)
3656 if (a < l->nbFeature.size())
3669 bool isvisible =
false;
3703 bool isvisible =
false;
3738 std::cout <<
" *********** Parsing XML for ME projection error ************ " << std::endl;
3740 xmlp.
parse(configFile);
void setFarClippingDistance(const double &dist)
void setNearClippingDistance(const double &dist)
unsigned int getCols() const
Type * data
Address of the first element of the data array.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
vpColVector & normalize()
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
static const vpColor green
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Class that defines generic functionalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
int getWindowXPosition() const
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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
int getWindowYPosition() const
static void flush(const vpImage< unsigned char > &I)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static FilterType getSobelKernelX(FilterType *filter, unsigned int size)
static FilterType getSobelKernelY(FilterType *filter, unsigned int size)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
static double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static double deg(double rad)
error that can be emitted by the vpMatrix class and its derivatives
@ incorrectMatrixSizeError
Incorrect matrix size.
Implementation of a matrix and operations on matrices.
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int size() const
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool isVisible(unsigned int i)
void addPolygon(PolygonType *p)
std::vector< PolygonType * > & getPolygon()
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void setOgreShowConfigDialog(bool showConfigDialog)
Main methods for a model-based tracker.
std::map< std::string, std::string > parseParameters(std::string &endLine)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
double m_lambda
Gain of the virtual visual servoing stage.
virtual vpColVector getEstimatedDoF() const
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
virtual void setEstimatedDoF(const vpColVector &v)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
void removeComment(std::ifstream &fileId)
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity, 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)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
void projectionErrorResetMovingEdges()
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
@ LEVENBERG_MARQUARDT_OPT
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, bool verbose=false, bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void init(const vpImage< unsigned char > &I)=0
virtual void computeVVSCheckLevenbergMarquardt(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)
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool useLodGeneral
True if LOD mode is enabled.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
std::map< std::string, std::string > mapOfParameterNames
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void loadVRMLModel(const std::string &modelFile)
unsigned int nbLines
Number of lines in CAO model.
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void savePose(const std::string &filename) const
void addPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, double minLineLengthThreshold=50.0)
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
vpMatrix covarianceMatrix
Covariance matrix.
vpHomogeneousMatrix m_cMo
The current pose.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
vpMatrix m_SobelX
Sobel kernel in X.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
unsigned int nbPoints
Number of points in CAO model.
vpCameraParameters m_cam
The camera parameters.
std::string modelFileName
bool useOgre
Use Ogre3d for visibility tests.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
virtual void setOgreVisibilityTest(const bool &v)
std::string poseSavingFilename
void setProjectionErrorKernelSize(const unsigned int &size)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
virtual void setLod(bool useLod, const std::string &name="")
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
double angleDisappears
Angle used to detect a face disappearance.
virtual void setNearClippingDistance(const double &dist)
void setProjectionErrorMovingEdge(const vpMe &me)
bool applyLodSettingInConfig
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
bool useScanLine
Use Scanline for visibility tests.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
vpMatrix m_SobelY
Sobel kernel in Y.
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
bool m_projectionErrorOgreShowConfigDialog
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
double distNearClip
Distance for near clipping.
bool m_sodb_init_called
Flag that indicates that SoDB::init(); was called.
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
unsigned int nbCylinders
Number of cylinders in CAO model.
unsigned int clippingFlag
Flags specifying which clipping to used.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
unsigned int nbCircles
Number of circles in CAO model.
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setVisible(bool _isvisible)
void setMovingEdge(vpMe *Me)
void setCameraParameters(const vpCameraParameters &camera)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
void setIndex(unsigned int i)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
double radius
The radius of the circle.
int index_polygon
Index of the faces which contain the line.
vpPoint * p3
An other point on the plane containing the circle.
vpMbtMeEllipse * meEllipse
The moving edge containers.
void setName(const std::string &circle_name)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Manage a cylinder used in the model-based tracker.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
unsigned int nbFeaturel2
The number of moving edges on line 2.
vpPoint * p2
The second extremity on the axe.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setMovingEdge(vpMe *Me)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
unsigned int nbFeature
The number of moving edges.
int index_polygon
Index of the face which contains the cylinder.
void setIndex(unsigned int i)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void setIndex(unsigned int i)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
vpMbtPolygon & getPolygon()
Implementation of a polygon of the model used by the model-based tracker.
void setMinPolygonAreaThresh(double min_polygon_area)
std::string getName() const
void setName(const std::string &face_name)
void setLod(bool use_lod)
virtual void setIndex(int i)
void setMinLineLengthThresh(double min_line_length)
void setIsPolygonOriented(const bool &oriented)
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void setProjectionErrorMe(const vpMe &me)
unsigned int getProjectionErrorKernelSize() const
void setProjectionErrorKernelSize(const unsigned int &size)
@ PROJECTION_ERROR_PARSER
void parse(const std::string &filename)
void getProjectionErrorMe(vpMe &me) const
void setVerbose(bool verbose)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
void set_x(double x)
Set the point x coordinate in the image plane.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_oY() const
Get the point oY coordinate in the object frame.
void setWorldCoordinates(double oX, double oY, double oZ)
void set_y(double y)
Set the point y coordinate in the image plane.
Implements a 3D polygon with render functionalities like clipping.
void setFarClippingDistance(const double &dist)
unsigned int getNbPoint() const
void setNearClippingDistance(const double &dist)
vpPoint * p
corners in the object frame
virtual void setNbPoint(unsigned int nb)
void setClipping(const unsigned int &flags)
void addPoint(unsigned int n, const vpPoint &P)
Defines a generic 2D polygon.
Implementation of a pose vector and operations on poses.
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void addPoint(const vpPoint &P)
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Implementation of a rotation vector as quaternion angle minimal representation.
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)