46 #include <visp3/core/vpConfig.h>
47 #if defined(VISP_HAVE_SIMDLIB)
48 #include <Simd/SimdLib.h>
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(VISP_HAVE_THREADS)
100 #ifndef DOXYGEN_SHOULD_SKIP_THIS
104 #if defined(VISP_HAVE_THREADS)
105 std::mutex g_mutex_cout;
112 SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) { }
114 std::vector<vpPoint> extremities;
117 double minLineLengthThresh;
124 struct PolygonFaceInfo
126 PolygonFaceInfo(
double dist,
const vpPolygon &poly,
const std::vector<vpPoint> &corners)
127 : distanceToCamera(dist), polygon(poly), faceCorners(corners)
130 bool operator<(
const PolygonFaceInfo &pfi)
const {
return distanceToCamera < pfi.distanceToCamera; }
132 double distanceToCamera;
134 std::vector<vpPoint> faceCorners;
144 std::istream &safeGetline(std::istream &is, std::string &t)
154 std::istream::sentry se(is,
true);
155 std::streambuf *sb = is.rdbuf();
158 int c = sb->sbumpc();
162 else if (c ==
'\r') {
163 if (sb->sgetc() ==
'\n')
167 else if (c == std::streambuf::traits_type::eof()) {
170 is.setstate(std::ios::eofbit);
187 : m_cam(), m_cMo(), oJo(6, 6), m_isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
188 computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
189 displayFeatures(false), m_optimizationMethod(
vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(
vpMath::rad(89)),
190 angleDisappears(
vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(
vpPolygon3D::NO_CLIPPING),
191 useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
192 nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
193 minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
194 m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
195 m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(),
196 m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5),
197 m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20),
198 m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(nullptr), m_I(), m_sodb_init_called(false),
238 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
246 #ifdef VISP_HAVE_MODULE_GUI
255 std::string ext =
".init";
256 std::string str_pose =
"";
257 size_t pos = initFile.rfind(ext);
260 std::fstream finitpos;
262 std::stringstream ss;
264 if (pos != std::string::npos)
265 str_pose = initFile.substr(0, pos) +
".0.pos";
267 str_pose = initFile +
".0.pos";
269 finitpos.open(str_pose.c_str(), std::ios::in);
276 if (finitpos.fail()) {
277 std::cout <<
"Cannot read " << ss.str() << std::endl <<
"cMo set to identity" << std::endl;
281 for (
unsigned int i = 0; i < 6; i += 1) {
282 finitpos >> init_pos[i];
288 std::cout <<
"Tracker initial pose read from " << ss.str() <<
": " << std::endl << last_cMo << std::endl;
303 std::cout <<
"No modification : left click " << std::endl;
304 std::cout <<
"Modify initial pose : right click " << std::endl;
345 ss.str(std::string());
351 if (pos != std::string::npos) {
359 std::cout <<
"Load 3D points from: " << ss.str() << std::endl;
360 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
361 finit.open(ss.str());
363 finit.open(ss.str().c_str());
366 std::cout <<
"Cannot read " << ss.str() << std::endl;
370 #ifdef VISP_HAVE_MODULE_IO
374 const std::string imgExtVec[] = {
".ppm",
".pgm",
".jpg",
".jpeg",
".png" };
376 bool foundHelpImg =
false;
377 if (pos != std::string::npos) {
378 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
379 dispF = initFile.substr(0, pos) + imgExtVec[i];
384 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
385 dispF = initFile + imgExtVec[i];
391 std::cout <<
"Load image to help initialization: " << dispF << std::endl;
392 #if defined(VISP_HAVE_X11)
394 #elif defined(VISP_HAVE_GDI)
396 #elif defined(HAVE_OPENCV_HIGHGUI)
402 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
406 d_help->init(Iref, winXPos + (
int)width + 80, winYPos,
"Where to initialize...");
414 if (d_help !=
nullptr) {
427 finit.ignore(256,
'\n');
428 std::cout <<
"Number of 3D points " << n3d << std::endl;
434 std::vector<vpPoint> P(n3d);
435 for (
unsigned int i = 0; i < n3d; i++) {
443 finit.ignore(256,
'\n');
446 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << pt_3d_tf[0] <<
" " << pt_3d_tf[1] <<
" "
447 << pt_3d_tf[2] << std::endl;
449 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
454 bool isWellInit =
false;
455 while (!isWellInit) {
456 std::vector<vpImagePoint> mem_ip;
457 for (
unsigned int i = 0; i < n3d; i++) {
458 std::ostringstream text;
459 text <<
"Click on point " << i + 1;
463 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
471 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
477 std::cout <<
"Click on point " << i + 1 <<
" ";
481 mem_ip.push_back(ip);
486 mem_ip.push_back(ip);
493 std::cout <<
"with 2D coordinates: " << ip << std::endl;
559 if (d_help !=
nullptr) {
565 std::cout <<
"cMo : " << std::endl <<
m_cMo << std::endl;
609 initClick(&I,
nullptr, initFile, displayHelp, T);
646 initClick(
nullptr, &I_color, initFile, displayHelp, T);
650 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile)
664 std::vector<vpPoint> P;
665 for (
unsigned int i = 0; i < points3D_list.size(); i++)
666 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
668 #ifdef VISP_HAVE_MODULE_IO
673 std::cout <<
"Load image to help initialization: " << displayFile << std::endl;
674 #if defined(VISP_HAVE_X11)
676 #elif defined(VISP_HAVE_GDI)
678 #elif defined VISP_HAVE_OPENCV
683 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
686 "Where to initialize...");
697 if (d_help !=
nullptr) {
708 bool isWellInit =
false;
709 while (!isWellInit) {
710 for (
unsigned int i = 0; i < points3D_list.size(); i++) {
711 std::cout <<
"Click on point " << i + 1 << std::endl;
727 std::cout <<
"Click on point " << ip << std::endl;
794 if (d_help !=
nullptr) {
819 const std::string &displayFile)
821 initClick(&I,
nullptr, points3D_list, displayFile);
836 const std::string &displayFile)
838 initClick(
nullptr, &I_color, points3D_list, displayFile);
843 const std::string &initFile)
845 std::stringstream ss;
848 std::string ext =
".init";
849 size_t pos = initFile.rfind(ext);
851 if (pos == initFile.size() - ext.size() && pos != 0) {
859 std::cout <<
"Load 2D/3D points from: " << ss.str() << std::endl;
860 finit.open(ss.str().c_str(), std::ios::in);
862 std::cout <<
"cannot read " << ss.str() << std::endl;
872 while (!finit.fail() && (c ==
'#')) {
873 finit.ignore(256,
'\n');
880 finit.ignore(256,
'\n');
881 std::cout <<
"Number of 3D points " << n3d << std::endl;
888 for (
unsigned int i = 0; i < n3d; i++) {
891 while (!finit.fail() && (c ==
'#')) {
892 finit.ignore(256,
'\n');
900 finit.ignore(256,
'\n');
902 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << X <<
" " << Y <<
" " << Z << std::endl;
911 while (!finit.fail() && (c ==
'#')) {
912 finit.ignore(256,
'\n');
919 finit.ignore(256,
'\n');
920 std::cout <<
"Number of 2D points " << n2d << std::endl;
930 "In %s file, number of 2D points %d and number of 3D "
931 "points %d are not equal",
932 ss.str().c_str(), n2d, n3d);
936 for (
unsigned int i = 0; i < n2d; i++) {
939 while (!finit.fail() && (c ==
'#')) {
940 finit.ignore(256,
'\n');
944 double u, v, x = 0, y = 0;
947 finit.ignore(256,
'\n');
950 std::cout <<
"Point " << i + 1 <<
" with 2D coordinates: " << ip << std::endl;
1031 const std::vector<vpImagePoint> &points2D_list,
1032 const std::vector<vpPoint> &points3D_list)
1034 if (points2D_list.size() != points3D_list.size())
1035 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points "
1036 "different to the number of 3D points.");
1038 size_t size = points3D_list.size();
1039 std::vector<vpPoint> P;
1042 for (
size_t i = 0; i < size; i++) {
1043 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1044 double x = 0, y = 0;
1071 const std::vector<vpPoint> &points3D_list)
1085 const std::vector<vpPoint> &points3D_list)
1091 const std::string &initFile)
1093 std::stringstream ss;
1097 std::string ext =
".pos";
1098 size_t pos = initFile.rfind(ext);
1100 if (pos == initFile.size() - ext.size() && pos != 0) {
1108 finit.open(ss.str().c_str(), std::ios::in);
1110 std::cout <<
"Cannot read " << ss.str() << std::endl;
1114 for (
unsigned int i = 0; i < 6; i += 1) {
1115 finit >> init_pos[i];
1233 std::fstream finitpos;
1234 finitpos.open(filename.c_str(), std::ios::out);
1237 finitpos << init_pos;
1242 bool useLod,
double minPolygonAreaThreshold,
double minLineLengthThreshold)
1244 std::vector<vpPoint> corners_without_duplicates;
1245 corners_without_duplicates.push_back(corners[0]);
1246 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
1247 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1248 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1249 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1250 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1251 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1252 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1253 corners_without_duplicates.push_back(corners[i + 1]);
1258 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
1280 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1281 polygon.
addPoint(j, corners_without_duplicates[j]);
1297 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold)
1330 y[0] = plane.
getA() / norm_Y;
1331 y[1] = plane.
getB() / norm_Y;
1332 y[2] = plane.
getC() / norm_Y;
1335 for (
unsigned int i = 0; i < 3; i++) {
1351 for (
unsigned int i = 0; i < 4; i++) {
1354 w_p = wMc * cMc_90 * c_p;
1377 bool useLod,
double minLineLengthThreshold)
1415 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
1418 for (
unsigned int i = 0; i < listFaces.size(); i++) {
1420 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
1421 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
1422 polygon.
addPoint(j, listFaces[i][j]);
1463 std::string::const_iterator it;
1466 it = modelFile.end();
1467 if ((*(it - 1) ==
'o' && *(it - 2) ==
'a' && *(it - 3) ==
'c' && *(it - 4) ==
'.') ||
1468 (*(it - 1) ==
'O' && *(it - 2) ==
'A' && *(it - 3) ==
'C' && *(it - 4) ==
'.')) {
1469 std::vector<std::string> vectorOfModelFilename;
1477 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose,
true, odTo);
1479 else if ((*(it - 1) ==
'l' && *(it - 2) ==
'r' && *(it - 3) ==
'w' && *(it - 4) ==
'.') ||
1480 (*(it - 1) ==
'L' && *(it - 2) ==
'R' && *(it - 3) ==
'W' && *(it - 4) ==
'.')) {
1515 #ifdef VISP_HAVE_COIN3D
1520 SbBool ok = in.openFile(modelFile.c_str());
1521 SoVRMLGroup *sceneGraphVRML2;
1528 if (!in.isFileVRML2()) {
1529 SoSeparator *sceneGraph = SoDB::readAll(&in);
1530 if (sceneGraph ==
nullptr) {
1534 SoToVRML2Action tovrml2;
1535 tovrml2.apply(sceneGraph);
1537 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1538 sceneGraphVRML2->ref();
1539 sceneGraph->unref();
1542 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1543 if (sceneGraphVRML2 ==
nullptr) {
1545 sceneGraphVRML2->ref();
1554 sceneGraphVRML2->unref();
1556 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1566 while (!fileId.fail() && (c ==
'#')) {
1567 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1570 if (fileId.fail()) {
1578 std::map<std::string, std::string> mapOfParams;
1581 while (!endLine.empty() && !exit) {
1587 std::string param(it->first +
"=");
1590 if (endLine.compare(0, param.size(), param) == 0) {
1592 endLine = endLine.substr(param.size());
1594 bool parseQuote =
false;
1595 if (it->second ==
"string") {
1597 if (endLine.size() > 2 && endLine[0] ==
'"') {
1599 endLine = endLine.substr(1);
1600 size_t pos = endLine.find_first_of(
'"');
1602 if (pos != std::string::npos) {
1603 mapOfParams[it->first] = endLine.substr(0, pos);
1604 endLine = endLine.substr(pos + 1);
1615 size_t pos1 = endLine.find_first_of(
' ');
1616 size_t pos2 = endLine.find_first_of(
'\t');
1617 size_t pos = pos1 < pos2 ? pos1 : pos2;
1619 mapOfParams[it->first] = endLine.substr(0, pos);
1620 endLine = endLine.substr(pos + 1);
1681 std::ifstream fileId;
1682 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1683 fileId.open(modelFile.c_str(), std::ifstream::in);
1684 if (fileId.fail()) {
1685 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
1690 std::cout <<
"Model file : " << modelFile << std::endl;
1692 vectorOfModelFilename.push_back(modelFile);
1705 fileId >> caoVersion;
1706 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1709 std::cout <<
"in vpMbTracker::loadCAOModel() -> Bad parameter header "
1710 "file : use V0, V1, ...";
1712 "header file : use V0, V1, ...");
1719 const std::string prefix_load =
"load";
1723 bool header =
false;
1724 while (c ==
'l' || c ==
'L') {
1725 getline(fileId, line);
1727 if (!line.compare(0, prefix_load.size(), prefix_load)) {
1729 std::string paramsStr = line.substr(5);
1731 paramsStr = paramsStr.substr(0, paramsStr.find_first_of(
")"));
1735 for (
size_t i = 0; i < params.size(); i++) {
1739 if (!params.empty()) {
1741 std::string headerPathRead = params[0];
1742 headerPathRead = headerPathRead.substr(1);
1743 headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of(
"\""));
1745 std::string headerPath = headerPathRead;
1760 for (
size_t i = 1; i < params.size(); i++) {
1761 std::string param = params[i];
1763 const std::string prefix =
"t=[";
1764 if (!param.compare(0, prefix.size(), prefix)) {
1765 param = param.substr(prefix.size());
1766 param = param.substr(0, param.find_first_of(
"]"));
1769 if (values.size() == 3) {
1770 t[0] = atof(values[0].c_str());
1771 t[1] = atof(values[1].c_str());
1772 t[2] = atof(values[2].c_str());
1777 const std::string prefix =
"tu=[";
1778 if (!param.compare(0, prefix.size(), prefix)) {
1779 param = param.substr(prefix.size());
1780 param = param.substr(0, param.find_first_of(
"]"));
1783 if (values.size() == 3) {
1784 for (
size_t j = 0; j < values.size(); j++) {
1785 std::string value = values[j];
1787 size_t unitPos = value.find(
"deg");
1788 if (unitPos != std::string::npos) {
1789 value = value.substr(0, unitPos);
1793 unitPos = value.find(
"rad");
1794 if (unitPos != std::string::npos) {
1795 value = value.substr(0, unitPos);
1797 tu[
static_cast<unsigned int>(j)] = !radian ?
vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1805 bool cyclic =
false;
1806 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1807 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1808 if (headerPath == *it) {
1816 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose,
false, odTo * oTo_local);
1823 std::cout <<
"WARNING Cyclic dependency detected with file " << headerPath <<
" declared in " << modelFile
1835 unsigned int caoNbrPoint;
1836 fileId >> caoNbrPoint;
1837 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1840 if (verbose || (parent && !header)) {
1841 #if defined(VISP_HAVE_THREADS)
1842 std::lock_guard<std::mutex> lock(g_mutex_cout);
1844 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
1847 if (caoNbrPoint > 100000) {
1851 if (caoNbrPoint == 0 && !header) {
1859 for (
unsigned int k = 0; k < caoNbrPoint; k++) {
1867 if (caoVersion == 2) {
1872 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1882 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1883 unsigned int caoNbrLine;
1884 fileId >> caoNbrLine;
1885 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1888 unsigned int *caoLinePoints =
nullptr;
1889 if (verbose || (parent && !header)) {
1890 #if defined(VISP_HAVE_THREADS)
1891 std::lock_guard<std::mutex> lock(g_mutex_cout);
1893 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
1896 if (caoNbrLine > 100000) {
1902 caoLinePoints =
new unsigned int[2 * caoNbrLine];
1904 unsigned int index1, index2;
1907 int idFace = startIdFace;
1909 for (
unsigned int k = 0; k < caoNbrLine; k++) {
1917 std::string endLine =
"";
1918 if (safeGetline(fileId, endLine).good()) {
1919 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1921 std::string segmentName =
"";
1924 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1925 segmentName = mapOfParams[
"name"];
1927 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1928 minLineLengthThresh = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1930 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1934 SegmentInfo segmentInfo;
1935 segmentInfo.name = segmentName;
1936 segmentInfo.useLod = useLod;
1937 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1939 caoLinePoints[2 * k] = index1;
1940 caoLinePoints[2 * k + 1] = index2;
1942 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1943 std::vector<vpPoint> extremities;
1944 extremities.push_back(caoPoints[index1]);
1945 extremities.push_back(caoPoints[index2]);
1946 segmentInfo.extremities = extremities;
1948 std::pair<unsigned int, unsigned int> key(index1, index2);
1950 segmentTemporaryMap[key] = segmentInfo;
1953 vpTRACE(
" line %d has wrong coordinates.", k);
1965 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1966 unsigned int caoNbrPolygonLine;
1967 fileId >> caoNbrPolygonLine;
1968 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1971 if (verbose || (parent && !header)) {
1972 #if defined(VISP_HAVE_THREADS)
1973 std::lock_guard<std::mutex> lock(g_mutex_cout);
1975 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon lines" << std::endl;
1978 if (caoNbrPolygonLine > 100000) {
1980 delete[] caoLinePoints;
1985 for (
unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1988 unsigned int nbLinePol;
1989 fileId >> nbLinePol;
1990 std::vector<vpPoint> corners;
1991 if (nbLinePol > 100000) {
1995 for (
unsigned int n = 0; n < nbLinePol; n++) {
1998 if (index >= caoNbrLine) {
2001 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2002 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2004 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2005 faceSegmentKeyVector.push_back(key);
2010 std::string endLine =
"";
2011 if (safeGetline(fileId, endLine).good()) {
2012 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2014 std::string polygonName =
"";
2017 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2018 polygonName = mapOfParams[
"name"];
2020 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2021 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2023 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2037 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2038 it != segmentTemporaryMap.end(); ++it) {
2039 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2040 faceSegmentKeyVector.end()) {
2042 it->second.minLineLengthThresh);
2055 unsigned int caoNbrPolygonPoint;
2056 fileId >> caoNbrPolygonPoint;
2057 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2060 if (verbose || (parent && !header)) {
2061 #if defined(VISP_HAVE_THREADS)
2062 std::lock_guard<std::mutex> lock(g_mutex_cout);
2064 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon points" << std::endl;
2067 if (caoNbrPolygonPoint > 100000) {
2071 for (
unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2074 unsigned int nbPointPol;
2075 fileId >> nbPointPol;
2076 if (nbPointPol > 100000) {
2079 std::vector<vpPoint> corners;
2080 for (
unsigned int n = 0; n < nbPointPol; n++) {
2082 if (index > caoNbrPoint - 1) {
2085 corners.push_back(caoPoints[index]);
2090 std::string endLine =
"";
2091 if (safeGetline(fileId, endLine).good()) {
2092 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2094 std::string polygonName =
"";
2097 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2098 polygonName = mapOfParams[
"name"];
2100 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2101 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2103 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2117 unsigned int caoNbCylinder;
2124 delete[] caoLinePoints;
2129 fileId >> caoNbCylinder;
2130 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2133 if (verbose || (parent && !header)) {
2134 #if defined(VISP_HAVE_THREADS)
2135 std::lock_guard<std::mutex> lock(g_mutex_cout);
2137 std::cout <<
"> " << caoNbCylinder <<
" cylinders" << std::endl;
2140 if (caoNbCylinder > 100000) {
2144 for (
unsigned int k = 0; k < caoNbCylinder; ++k) {
2148 unsigned int indexP1, indexP2;
2155 std::string endLine =
"";
2156 if (safeGetline(fileId, endLine).good()) {
2157 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2159 std::string polygonName =
"";
2162 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2163 polygonName = mapOfParams[
"name"];
2165 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
2166 minLineLengthThreshold = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
2168 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2172 int idRevolutionAxis = idFace;
2173 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2176 minLineLengthThreshold);
2178 std::vector<std::vector<vpPoint> > listFaces;
2180 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2182 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2192 catch (
const std::exception &e) {
2193 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2194 std::cerr <<
"Exception: " << e.what() << std::endl;
2199 unsigned int caoNbCircle;
2206 delete[] caoLinePoints;
2211 fileId >> caoNbCircle;
2212 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2215 if (verbose || (parent && !header)) {
2216 #if defined(VISP_HAVE_THREADS)
2217 std::lock_guard<std::mutex> lock(g_mutex_cout);
2219 std::cout <<
"> " << caoNbCircle <<
" circles" << std::endl;
2222 if (caoNbCircle > 100000) {
2226 for (
unsigned int k = 0; k < caoNbCircle; ++k) {
2230 unsigned int indexP1, indexP2, indexP3;
2238 std::string endLine =
"";
2239 if (safeGetline(fileId, endLine).good()) {
2240 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2242 std::string polygonName =
"";
2245 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2246 polygonName = mapOfParams[
"name"];
2248 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2249 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2251 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2255 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2256 minPolygonAreaThreshold);
2258 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2261 polygonName, useLod, minPolygonAreaThreshold);
2268 catch (
const std::exception &e) {
2269 std::cerr <<
"Cannot get the number of circles. Defaulting to zero." << std::endl;
2270 std::cerr <<
"Exception: " << e.what() << std::endl;
2274 startIdFace = idFace;
2277 delete[] caoLinePoints;
2279 if (header && parent) {
2281 #if defined(VISP_HAVE_THREADS)
2282 std::lock_guard<std::mutex> lock(g_mutex_cout);
2284 std::cout <<
"Global information for " <<
vpIoTools::getName(modelFile) <<
" :" << std::endl;
2285 std::cout <<
"Total nb of points : " <<
nbPoints << std::endl;
2286 std::cout <<
"Total nb of lines : " <<
nbLines << std::endl;
2287 std::cout <<
"Total nb of polygon lines : " <<
nbPolygonLines << std::endl;
2288 std::cout <<
"Total nb of polygon points : " <<
nbPolygonPoints << std::endl;
2289 std::cout <<
"Total nb of cylinders : " <<
nbCylinders << std::endl;
2290 std::cout <<
"Total nb of circles : " <<
nbCircles << std::endl;
2293 #if defined(VISP_HAVE_THREADS)
2294 std::lock_guard<std::mutex> lock(g_mutex_cout);
2296 std::cout <<
"> " <<
nbPoints <<
" points" << std::endl;
2297 std::cout <<
"> " <<
nbLines <<
" lines" << std::endl;
2298 std::cout <<
"> " <<
nbPolygonLines <<
" polygon lines" << std::endl;
2299 std::cout <<
"> " <<
nbPolygonPoints <<
" polygon points" << std::endl;
2300 std::cout <<
"> " <<
nbCylinders <<
" cylinders" << std::endl;
2301 std::cout <<
"> " <<
nbCircles <<
" circles" << std::endl;
2306 vectorOfModelFilename.pop_back();
2308 catch (
const std::exception &e) {
2309 std::cerr <<
"Cannot read line!" << std::endl;
2310 std::cerr <<
"Exception: " << e.what() << std::endl;
2315 #ifdef VISP_HAVE_COIN3D
2326 SoVRMLTransform *sceneGraphVRML2Trasnform =
dynamic_cast<SoVRMLTransform *
>(sceneGraphVRML2);
2327 if (sceneGraphVRML2Trasnform) {
2328 float rx, ry, rz, rw;
2329 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2335 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2336 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2337 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2343 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2344 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2345 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2349 for (
unsigned int i = 0; i < 3; i++)
2351 for (
unsigned int i = 0; i < 3; i++)
2353 for (
unsigned int i = 0; i < 3; i++)
2357 transform = transform * transformCur;
2360 int nbShapes = sceneGraphVRML2->getNumChildren();
2367 for (
int i = 0; i < nbShapes; i++) {
2369 child = sceneGraphVRML2->getChild(i);
2371 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2372 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2375 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2376 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2379 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2380 SoChildList *child2list = child->getChildren();
2381 std::string name = child->getName().getString();
2383 for (
int j = 0; j < child2list->getLength(); j++) {
2384 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2385 SoVRMLIndexedFaceSet *face_set;
2386 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2387 if (!strncmp(face_set->getName().getString(),
"cyl", 3)) {
2394 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2395 SoVRMLIndexedLineSet *line_set;
2396 line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2414 const std::string &polygonName)
2416 std::vector<vpPoint> corners;
2420 int indexListSize = face_set->coordIndex.getNum();
2424 SoVRMLCoordinate *coord;
2426 for (
int i = 0; i < indexListSize; i++) {
2427 if (face_set->coordIndex[i] == -1) {
2428 if (corners.size() > 1) {
2438 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2439 int index = face_set->coordIndex[i];
2440 pointTransformed[0] = coord->point[index].getValue()[0];
2441 pointTransformed[1] = coord->point[index].getValue()[1];
2442 pointTransformed[2] = coord->point[index].getValue()[2];
2443 pointTransformed[3] = 1.0;
2445 pointTransformed = transform * pointTransformed;
2448 corners.push_back(pt);
2468 const std::string &polygonName)
2470 std::vector<vpPoint> corners_c1, corners_c2;
2473 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2475 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
2477 if (indexListSize % 2 == 1) {
2478 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
2481 corners_c1.resize(indexListSize / 2);
2482 corners_c2.resize(indexListSize / 2);
2488 for (
int i = 0; i < coords->point.getNum(); ++i) {
2489 pointTransformed[0] = coords->point[i].getValue()[0];
2490 pointTransformed[1] = coords->point[i].getValue()[1];
2491 pointTransformed[2] = coords->point[i].getValue()[2];
2492 pointTransformed[3] = 1.0;
2494 pointTransformed = transform * pointTransformed;
2498 if (i < (
int)corners_c1.size()) {
2499 corners_c1[(
unsigned int)i] = pt;
2502 corners_c2[(
unsigned int)i - corners_c1.size()] = pt;
2510 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
2511 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
2512 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
2513 double radius_c1 = sqrt(dist.
sumSquare());
2514 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
2515 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
2516 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
2517 double radius_c2 = sqrt(dist.
sumSquare());
2519 if (std::fabs(radius_c1 - radius_c2) >
2520 (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))) {
2521 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
2528 int idRevolutionAxis = idFace;
2533 std::vector<std::vector<vpPoint> > listFaces;
2537 initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2555 std::vector<vpPoint> corners;
2558 int indexListSize = line_set->coordIndex.getNum();
2560 SbVec3f point(0, 0, 0);
2562 SoVRMLCoordinate *coord;
2564 for (
int i = 0; i < indexListSize; i++) {
2565 if (line_set->coordIndex[i] == -1) {
2566 if (corners.size() > 1) {
2576 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2577 int index = line_set->coordIndex[i];
2578 point[0] = coord->point[index].getValue()[0];
2579 point[1] = coord->point[index].getValue()[1];
2580 point[2] = coord->point[index].getValue()[2];
2583 corners.push_back(pt);
2602 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
2610 for (
unsigned int i = 0; i < pts.size(); ++i) {
2612 oY += pts[i].get_oY();
2613 oZ += pts[i].get_oZ();
2632 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2636 std::vector<vpPolygon> polygonsTmp;
2637 std::vector<std::vector<vpPoint> > roisPtTmp;
2640 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2645 if ((useVisibility &&
faces.
getPolygon()[i]->isvisible) || !useVisibility) {
2646 std::vector<vpImagePoint> roiPts;
2655 if (roiPts.size() <= 2) {
2659 polygonsTmp.push_back(
vpPolygon(roiPts));
2661 std::vector<vpPoint> polyPts;
2670 roisPtTmp.push_back(polyPts);
2675 if (orderPolygons) {
2677 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2678 for (
unsigned int i = 0; i < polygonsTmp.size(); i++) {
2679 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2680 for (
unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2681 x_centroid += roisPtTmp[i][j].get_X();
2682 y_centroid += roisPtTmp[i][j].get_Y();
2683 z_centroid += roisPtTmp[i][j].get_Z();
2686 x_centroid /= roisPtTmp[i].size();
2687 y_centroid /= roisPtTmp[i].size();
2688 z_centroid /= roisPtTmp[i].size();
2690 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2691 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2695 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2697 polygonsTmp.resize(listOfPolygonFaces.size());
2698 roisPtTmp.resize(listOfPolygonFaces.size());
2701 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2703 polygonsTmp[cpt] = it->polygon;
2704 roisPtTmp[cpt] = it->faceCorners;
2707 pairOfPolygonFaces.first = polygonsTmp;
2708 pairOfPolygonFaces.second = roisPtTmp;
2711 pairOfPolygonFaces.first = polygonsTmp;
2712 pairOfPolygonFaces.second = roisPtTmp;
2715 return pairOfPolygonFaces;
2730 #ifndef VISP_HAVE_OGRE
2732 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2733 "will be used. setOgreVisibilityTest() set to false."
2747 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. "
2748 "Far clipping won't be considered.");
2750 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping "
2751 "won't be considered.");
2755 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2758 #ifdef VISP_HAVE_OGRE
2776 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2777 if (name.empty() ||
faces[i]->name == name) {
2778 faces[i]->setLod(useLod);
2794 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2795 if (name.empty() ||
faces[i]->name == name) {
2796 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2811 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2812 if (name.empty() ||
faces[i]->name == name) {
2813 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2826 vpTRACE(
"Near clipping value cannot be superior than far clipping value. "
2827 "Near clipping won't be considered.");
2829 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping "
2830 "won't be considered.");
2834 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2837 #ifdef VISP_HAVE_OGRE
2853 for (
unsigned int i = 0; i <
faces.
size(); i++)
2867 if (isoJoIdentity) {
2895 #if defined(VISP_HAVE_SIMDLIB)
2898 const unsigned int N = interaction.
getRows();
2900 for (
unsigned int i = 0; i < 6; i += 1) {
2902 for (
unsigned int j = 0; j < N; j += 1) {
2903 ssum += interaction[j][i] * error[j];
2912 double &mu,
bool &reStartFromLastIncrement,
vpColVector *
const w,
2923 error = m_error_prev;
2924 if (w !=
nullptr && m_w_prev !=
nullptr) {
2927 reStartFromLastIncrement =
true;
2937 if (isoJoIdentity) {
2945 vpMatrix LTLmuI = LTL + (LMA * mu);
2952 if (w !=
nullptr && m_w_prev !=
nullptr)
2975 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2983 if (w !=
nullptr && m_w_prev !=
nullptr)
3016 for (
unsigned int i = 0; i < 6; i++)
3040 for (
unsigned int i = 0; i < 6; i++) {
3042 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3054 std::vector<std::vector<vpPoint> > &listFaces)
3076 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3080 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3084 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon())
3114 std::vector<vpPoint> pointsFace;
3115 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3116 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3117 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3118 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3119 listFaces.push_back(pointsFace);
3122 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3123 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3124 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3125 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3126 listFaces.push_back(pointsFace);
3129 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3130 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3131 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3132 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3133 listFaces.push_back(pointsFace);
3136 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3137 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3138 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3139 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3140 listFaces.push_back(pointsFace);
3158 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3159 dz <= std::numeric_limits<double>::epsilon())
3166 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold,
3167 double minLineLengthThreshold)
3169 std::vector<vpPoint> corners_without_duplicates;
3170 corners_without_duplicates.push_back(corners[0]);
3171 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
3172 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3173 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3174 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3175 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3176 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3177 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3178 corners_without_duplicates.push_back(corners[i + 1]);
3183 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
3191 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3192 polygon.
addPoint(j, corners_without_duplicates[j]);
3208 int idFace,
const std::string &polygonName,
bool useLod,
3209 double minPolygonAreaThreshold)
3236 y[0] = plane.
getA() / norm_Y;
3237 y[1] = plane.
getB() / norm_Y;
3238 y[2] = plane.
getC() / norm_Y;
3241 for (
unsigned int i = 0; i < 3; i++) {
3257 for (
unsigned int i = 0; i < 4; i++) {
3260 w_p = wMc * cMc_90 * c_p;
3283 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3315 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3318 for (
unsigned int i = 0; i < listFaces.size(); i++) {
3320 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
3321 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
3322 polygon.
addPoint(j, listFaces[i][j]);
3349 bool already_here =
false;
3356 already_here =
true;
3362 if (!already_here) {
3389 int idFace,
const std::string &name)
3391 bool already_here =
false;
3404 if (!already_here) {
3420 const std::string &name)
3422 bool already_here =
false;
3435 if (!already_here) {
3450 int idFace,
const std::string &name)
3456 const std::string &name)
3465 for (
unsigned int i = 0; i < nbpt - 1; i++)
3475 for (
unsigned int i = 0; i < nbpt - 1; i++)
3504 unsigned int nbFeatures = 0;
3507 if (nbFeatures > 0) {
3508 return vpMath::deg(totalProjectionError / (
double)nbFeatures);
3540 #ifdef VISP_HAVE_OGRE
3571 double totalProjectionError = 0.0;
3576 for (
size_t a = 0; a < l->
meline.size(); a++) {
3577 if (l->
meline[a] !=
nullptr) {
3578 double lineNormGradient;
3579 unsigned int lineNbFeatures;
3583 totalProjectionError += lineNormGradient;
3584 nbFeatures += lineNbFeatures;
3595 double cylinderNormGradient = 0;
3596 unsigned int cylinderNbFeatures = 0;
3600 totalProjectionError += cylinderNormGradient;
3601 nbFeatures += cylinderNbFeatures;
3605 double cylinderNormGradient = 0;
3606 unsigned int cylinderNbFeatures = 0;
3610 totalProjectionError += cylinderNormGradient;
3611 nbFeatures += cylinderNbFeatures;
3620 double circleNormGradient = 0;
3621 unsigned int circleNbFeatures = 0;
3625 totalProjectionError += circleNormGradient;
3626 nbFeatures += circleNbFeatures;
3630 return totalProjectionError;
3635 bool changed =
false;
3642 #ifdef VISP_HAVE_OGRE
3656 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
3657 if ((*it)->meline[a] !=
nullptr) {
3658 delete (*it)->meline[a];
3659 (*it)->meline[a] =
nullptr;
3663 (*it)->meline.clear();
3664 (*it)->nbFeature.clear();
3665 (*it)->nbFeatureTotal = 0;
3670 if ((*it)->meline1 !=
nullptr) {
3671 delete (*it)->meline1;
3672 (*it)->meline1 =
nullptr;
3674 if ((*it)->meline2 !=
nullptr) {
3675 delete (*it)->meline2;
3676 (*it)->meline2 =
nullptr;
3679 (*it)->nbFeature = 0;
3680 (*it)->nbFeaturel1 = 0;
3681 (*it)->nbFeaturel2 = 0;
3686 if ((*it)->meEllipse !=
nullptr) {
3687 delete (*it)->meEllipse;
3688 (*it)->meEllipse =
nullptr;
3690 (*it)->nbFeature = 0;
3696 const bool doNotTrack =
true;
3701 bool isvisible =
false;
3705 int index = *itindex;
3726 for (
size_t a = 0; a < l->
meline.size(); a++) {
3727 if (l->
meline[a] !=
nullptr)
3729 if (a < l->nbFeature.size())
3742 bool isvisible =
false;
3777 bool isvisible =
false;
3806 #if defined(VISP_HAVE_PUGIXML)
3814 std::cout <<
" *********** Parsing XML for ME projection error ************ " << std::endl;
3816 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)
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 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
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
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.
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=nullptr, vpColVector *const m_w_prev=nullptr)
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.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
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)
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)
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)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
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.
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 initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
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 &)=nullptr)
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)