50#include <Simd/SimdLib.hpp>
52#include <visp3/core/vpColVector.h>
53#include <visp3/core/vpDisplay.h>
54#include <visp3/core/vpMath.h>
55#include <visp3/core/vpMatrix.h>
56#include <visp3/core/vpPoint.h>
57#include <visp3/vision/vpPose.h>
58#ifdef VISP_HAVE_MODULE_GUI
59#include <visp3/gui/vpDisplayGDI.h>
60#include <visp3/gui/vpDisplayOpenCV.h>
61#include <visp3/gui/vpDisplayX.h>
63#include <visp3/core/vpCameraParameters.h>
64#include <visp3/core/vpColor.h>
65#include <visp3/core/vpException.h>
66#include <visp3/core/vpIoTools.h>
67#include <visp3/core/vpPixelMeterConversion.h>
68#ifdef VISP_HAVE_MODULE_IO
69#include <visp3/io/vpImageIo.h>
71#include <visp3/core/vpCPUFeatures.h>
72#include <visp3/core/vpIoTools.h>
73#include <visp3/core/vpMatrixException.h>
74#include <visp3/core/vpTrackingException.h>
75#include <visp3/mbt/vpMbTracker.h>
77#include <visp3/core/vpImageFilter.h>
78#include <visp3/mbt/vpMbtXmlGenericParser.h>
80#ifdef VISP_HAVE_COIN3D
82#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
83#include <Inventor/VRMLnodes/SoVRMLGroup.h>
84#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
85#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
86#include <Inventor/VRMLnodes/SoVRMLShape.h>
87#include <Inventor/VRMLnodes/SoVRMLTransform.h>
88#include <Inventor/actions/SoGetMatrixAction.h>
89#include <Inventor/actions/SoGetPrimitiveCountAction.h>
90#include <Inventor/actions/SoSearchAction.h>
91#include <Inventor/actions/SoToVRML2Action.h>
92#include <Inventor/actions/SoWriteAction.h>
93#include <Inventor/misc/SoChildList.h>
94#include <Inventor/nodes/SoSeparator.h>
97#ifndef DOXYGEN_SHOULD_SKIP_THIS
105 SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
107 std::vector<vpPoint> extremities;
110 double minLineLengthThresh;
117struct PolygonFaceInfo {
118 PolygonFaceInfo(
double dist,
const vpPolygon &poly,
const std::vector<vpPoint> &corners)
119 : distanceToCamera(dist), polygon(poly), faceCorners(corners)
123 bool operator<(
const PolygonFaceInfo &pfi)
const {
return distanceToCamera < pfi.distanceToCamera; }
125 double distanceToCamera;
127 std::vector<vpPoint> faceCorners;
137std::istream& safeGetline(std::istream& is, std::string& t)
147 std::istream::sentry se(is,
true);
148 std::streambuf* sb = is.rdbuf();
151 int c = sb->sbumpc();
155 else if (c ==
'\r') {
156 if(sb->sgetc() ==
'\n')
160 else if (c == std::streambuf::traits_type::eof()) {
163 is.setstate(std::ios::eofbit);
180 : m_cam(), m_cMo(), oJo(6, 6), 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(),
189 m_projectionErrorFaces(), m_projectionErrorOgreShowConfigDialog(false),
190 m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5,5), m_SobelY(5,5),
191 m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), m_projectionErrorDisplayThickness(1),
192 m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false), m_rand()
227#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
235#ifdef VISP_HAVE_MODULE_GUI
244 std::string ext =
".init";
245 std::string str_pose =
"";
246 size_t pos = initFile.rfind(ext);
249 std::fstream finitpos;
251 char s[FILENAME_MAX];
253 if (pos != std::string::npos)
254 str_pose = initFile.substr(0, pos) +
".0.pos";
256 str_pose = initFile +
".0.pos";
258 finitpos.open(str_pose.c_str(), std::ios::in);
259 sprintf(s,
"%s", str_pose.c_str());
264 if (finitpos.fail()) {
265 std::cout <<
"cannot read " << s << std::endl <<
"cMo set to identity" << std::endl;
268 for (
unsigned int i = 0; i < 6; i += 1) {
269 finitpos >> init_pos[i];
275 std::cout <<
"last_cMo : " << std::endl << last_cMo << std::endl;
289 std::cout <<
"No modification : left click " << std::endl;
290 std::cout <<
"Modify initial pose : right click " << std::endl;
332 if (pos != std::string::npos)
333 sprintf(s,
"%s", initFile.c_str());
335 sprintf(s,
"%s.init", initFile.c_str());
337 std::cout <<
"Load 3D points from: " << s << std::endl;
340 std::cout <<
"cannot read " << s << std::endl;
344#ifdef VISP_HAVE_MODULE_IO
348 const std::string imgExtVec[] = {
".ppm",
".pgm",
".jpg",
".jpeg",
".png"};
350 bool foundHelpImg =
false;
351 if (pos != std::string::npos) {
352 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
353 dispF = initFile.substr(0, pos) + imgExtVec[i];
357 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
358 dispF = initFile + imgExtVec[i];
364 std::cout <<
"Load image to help initialization: " << dispF << std::endl;
365#if defined VISP_HAVE_X11
367#elif defined VISP_HAVE_GDI
369#elif defined VISP_HAVE_OPENCV
375#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
379 d_help->init(Iref, winXPos + (
int)width + 80, winYPos,
380 "Where to initialize...");
387 if (d_help != NULL) {
400 finit.ignore(256,
'\n');
401 std::cout <<
"Number of 3D points " << n3d << std::endl;
406 std::vector<vpPoint> P(n3d);
407 for (
unsigned int i = 0; i < n3d; i++) {
415 finit.ignore(256,
'\n');
418 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << pt_3d_tf[0] <<
" " << pt_3d_tf[1] <<
" " << pt_3d_tf[2] << std::endl;
420 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
425 bool isWellInit =
false;
426 while (!isWellInit) {
427 std::vector<vpImagePoint> mem_ip;
428 for (
unsigned int i = 0; i < n3d; i++) {
429 std::ostringstream text;
430 text <<
"Click on point " << i + 1;
434 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
441 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
447 std::cout <<
"Click on point " << i + 1 <<
" ";
451 mem_ip.push_back(ip);
455 mem_ip.push_back(ip);
462 std::cout <<
"with 2D coordinates: " << ip << std::endl;
476 d1 = d2 = std::numeric_limits<double>::max();
546 if (d_help != NULL) {
552 std::cout <<
"cMo : " << std::endl <<
m_cMo << std::endl;
596 initClick(&I, NULL, initFile, displayHelp, T);
633 initClick(NULL, &I_color, initFile, displayHelp, T);
637 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile)
650 std::vector<vpPoint> P;
651 for (
unsigned int i = 0; i < points3D_list.size(); i++)
652 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
654#ifdef VISP_HAVE_MODULE_IO
659 std::cout <<
"Load image to help initialization: " << displayFile << std::endl;
660#if defined VISP_HAVE_X11
662#elif defined VISP_HAVE_GDI
664#elif defined VISP_HAVE_OPENCV
669#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
672 "Where to initialize...");
675 "Where to initialize...");
681 if (d_help != NULL) {
692 bool isWellInit =
false;
693 while (!isWellInit) {
694 for (
unsigned int i = 0; i < points3D_list.size(); i++) {
695 std::cout <<
"Click on point " << i + 1 << std::endl;
710 std::cout <<
"Click on point " << ip << std::endl;
727 d1 = d2 = std::numeric_limits<double>::max();
793 if (d_help != NULL) {
818 const std::string &displayFile)
820 initClick(&I, NULL, points3D_list, displayFile);
835 const std::string &displayFile)
837 initClick(NULL, &I_color, points3D_list, displayFile);
842 const std::string &initFile)
844 char s[FILENAME_MAX];
847 std::string ext =
".init";
848 size_t pos = initFile.rfind(ext);
850 if (pos == initFile.size() - ext.size() && pos != 0)
851 sprintf(s,
"%s", initFile.c_str());
853 sprintf(s,
"%s.init", initFile.c_str());
855 std::cout <<
"Load 2D/3D points from: " << s << std::endl;
856 finit.open(s, std::ios::in);
858 std::cout <<
"cannot read " << s << std::endl;
868 while (!finit.fail() && (c ==
'#')) {
869 finit.ignore(256,
'\n');
876 finit.ignore(256,
'\n');
877 std::cout <<
"Number of 3D points " << n3d << std::endl;
883 for (
unsigned int i = 0; i < n3d; i++) {
886 while (!finit.fail() && (c ==
'#')) {
887 finit.ignore(256,
'\n');
895 finit.ignore(256,
'\n');
897 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << X <<
" " << Y <<
" " << Z << std::endl;
906 while (!finit.fail() && (c ==
'#')) {
907 finit.ignore(256,
'\n');
914 finit.ignore(256,
'\n');
915 std::cout <<
"Number of 2D points " << n2d << std::endl;
924 "In %s file, number of 2D points %d and number of 3D "
925 "points %d are not equal",
930 for (
unsigned int i = 0; i < n2d; i++) {
933 while (!finit.fail() && (c ==
'#')) {
934 finit.ignore(256,
'\n');
938 double u, v, x = 0, y = 0;
941 finit.ignore(256,
'\n');
944 std::cout <<
"Point " << i + 1 <<
" with 2D coordinates: " << ip << std::endl;
955 d1 = d2 = std::numeric_limits<double>::max();
1047 const std::vector<vpImagePoint> &points2D_list,
const std::vector<vpPoint> &points3D_list)
1049 if (points2D_list.size() != points3D_list.size())
1050 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points "
1051 "different to the number of 3D points.");
1053 size_t size = points3D_list.size();
1054 std::vector<vpPoint> P;
1057 for (
size_t i = 0; i < size; i++) {
1058 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1059 double x = 0, y = 0;
1068 d1 = d2 = std::numeric_limits<double>::max();
1108 const std::vector<vpPoint> &points3D_list)
1122 const std::vector<vpPoint> &points3D_list)
1128 const std::string &initFile)
1130 char s[FILENAME_MAX];
1134 std::string ext =
".pos";
1135 size_t pos = initFile.rfind(ext);
1137 if (pos == initFile.size() - ext.size() && pos != 0)
1138 sprintf(s,
"%s", initFile.c_str());
1140 sprintf(s,
"%s.pos", initFile.c_str());
1142 finit.open(s, std::ios::in);
1144 std::cout <<
"cannot read " << s << std::endl;
1148 for (
unsigned int i = 0; i < 6; i += 1) {
1149 finit >> init_pos[i];
1266 std::fstream finitpos;
1267 char s[FILENAME_MAX];
1269 sprintf(s,
"%s", filename.c_str());
1270 finitpos.open(s, std::ios::out);
1273 finitpos << init_pos;
1278 bool useLod,
double minPolygonAreaThreshold,
1279 double minLineLengthThreshold)
1281 std::vector<vpPoint> corners_without_duplicates;
1282 corners_without_duplicates.push_back(corners[0]);
1283 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
1284 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1285 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1286 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1287 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1288 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1289 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1290 corners_without_duplicates.push_back(corners[i + 1]);
1295 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
1317 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1318 polygon.
addPoint(j, corners_without_duplicates[j]);
1334 int idFace,
const std::string &polygonName,
bool useLod,
1335 double minPolygonAreaThreshold)
1368 y[0] = plane.
getA() / norm_Y;
1369 y[1] = plane.
getB() / norm_Y;
1370 y[2] = plane.
getC() / norm_Y;
1373 for (
unsigned int i = 0; i < 3; i++) {
1389 for (
unsigned int i = 0; i < 4; i++) {
1392 w_p = wMc * cMc_90 * c_p;
1415 bool useLod,
double minLineLengthThreshold)
1453 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
1456 for (
unsigned int i = 0; i < listFaces.size(); i++) {
1458 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
1459 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
1460 polygon.
addPoint(j, listFaces[i][j]);
1501 std::string::const_iterator it;
1504 it = modelFile.end();
1505 if ((*(it - 1) ==
'o' && *(it - 2) ==
'a' && *(it - 3) ==
'c' && *(it - 4) ==
'.') ||
1506 (*(it - 1) ==
'O' && *(it - 2) ==
'A' && *(it - 3) ==
'C' && *(it - 4) ==
'.')) {
1507 std::vector<std::string> vectorOfModelFilename;
1515 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose,
true, odTo);
1516 }
else if ((*(it - 1) ==
'l' && *(it - 2) ==
'r' && *(it - 3) ==
'w' && *(it - 4) ==
'.') ||
1517 (*(it - 1) ==
'L' && *(it - 2) ==
'R' && *(it - 3) ==
'W' && *(it - 4) ==
'.')) {
1550#ifdef VISP_HAVE_COIN3D
1555 SbBool ok = in.openFile(modelFile.c_str());
1556 SoVRMLGroup *sceneGraphVRML2;
1563 if (!in.isFileVRML2()) {
1564 SoSeparator *sceneGraph = SoDB::readAll(&in);
1565 if (sceneGraph == NULL) {
1569 SoToVRML2Action tovrml2;
1570 tovrml2.apply(sceneGraph);
1572 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1573 sceneGraphVRML2->ref();
1574 sceneGraph->unref();
1576 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1577 if (sceneGraphVRML2 == NULL) {
1579 sceneGraphVRML2->ref();
1588 sceneGraphVRML2->unref();
1590 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1600 while (!fileId.fail() && (c ==
'#')) {
1601 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1604 if (fileId.fail()) {
1612 std::map<std::string, std::string> mapOfParams;
1615 while (!endLine.empty() && !exit) {
1621 std::string param(it->first +
"=");
1624 if (endLine.compare(0, param.size(), param) == 0) {
1626 endLine = endLine.substr(param.size());
1628 bool parseQuote =
false;
1629 if (it->second ==
"string") {
1631 if (endLine.size() > 2 && endLine[0] ==
'"') {
1633 endLine = endLine.substr(1);
1634 size_t pos = endLine.find_first_of(
'"');
1636 if (pos != std::string::npos) {
1637 mapOfParams[it->first] = endLine.substr(0, pos);
1638 endLine = endLine.substr(pos + 1);
1648 size_t pos1 = endLine.find_first_of(
' ');
1649 size_t pos2 = endLine.find_first_of(
'\t');
1650 size_t pos = pos1 < pos2 ? pos1 : pos2;
1652 mapOfParams[it->first] = endLine.substr(0, pos);
1653 endLine = endLine.substr(pos + 1);
1712 int &startIdFace,
bool verbose,
bool parent,
1715 std::ifstream fileId;
1716 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1717 fileId.open(modelFile.c_str(), std::ifstream::in);
1718 if (fileId.fail()) {
1719 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
1724 std::cout <<
"Model file : " << modelFile << std::endl;
1726 vectorOfModelFilename.push_back(modelFile);
1739 fileId >> caoVersion;
1740 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1742 std::cout <<
"in vpMbTracker::loadCAOModel() -> Bad parameter header "
1743 "file : use V0, V1, ...";
1745 "header file : use V0, V1, ...");
1752 const std::string prefix_load =
"load";
1756 bool header =
false;
1757 while (c ==
'l' || c ==
'L') {
1758 getline(fileId, line);
1760 if (!line.compare(0, prefix_load.size(), prefix_load)) {
1762 std::string paramsStr = line.substr(5);
1764 paramsStr = paramsStr.substr(0, paramsStr.find_first_of(
")"));
1768 for (
size_t i = 0; i < params.size(); i++) {
1772 if (!params.empty()) {
1774 std::string headerPathRead = params[0];
1775 headerPathRead = headerPathRead.substr(1);
1776 headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of(
"\""));
1778 std::string headerPath = headerPathRead;
1793 for (
size_t i = 1; i < params.size(); i++) {
1794 std::string param = params[i];
1796 const std::string prefix =
"t=[";
1797 if (!param.compare(0, prefix.size(), prefix)) {
1798 param = param.substr(prefix.size());
1799 param = param.substr(0, param.find_first_of(
"]"));
1802 if (values.size() == 3) {
1803 t[0] = atof(values[0].c_str());
1804 t[1] = atof(values[1].c_str());
1805 t[2] = atof(values[2].c_str());
1810 const std::string prefix =
"tu=[";
1811 if (!param.compare(0, prefix.size(), prefix)) {
1812 param = param.substr(prefix.size());
1813 param = param.substr(0, param.find_first_of(
"]"));
1816 if (values.size() == 3) {
1817 for (
size_t j = 0; j < values.size(); j++) {
1818 std::string value = values[j];
1820 size_t unitPos = value.find(
"deg");
1821 if (unitPos != std::string::npos) {
1822 value = value.substr(0, unitPos);
1826 unitPos = value.find(
"rad");
1827 if (unitPos != std::string::npos) {
1828 value = value.substr(0, unitPos);
1830 tu[
static_cast<unsigned int>(j)] = !radian ?
vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1838 bool cyclic =
false;
1839 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1840 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1841 if (headerPath == *it) {
1849 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose,
false, odTo*oTo_local);
1854 std::cout <<
"WARNING Cyclic dependency detected with file " << headerPath <<
" declared in " << modelFile
1866 unsigned int caoNbrPoint;
1867 fileId >> caoNbrPoint;
1868 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1871 if (verbose || (parent && !header)) {
1872 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
1875 if (caoNbrPoint > 100000) {
1879 if (caoNbrPoint == 0 && !header) {
1887 for (
unsigned int k = 0; k < caoNbrPoint; k++) {
1895 if (caoVersion == 2) {
1900 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1910 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1911 unsigned int caoNbrLine;
1912 fileId >> caoNbrLine;
1913 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1916 unsigned int *caoLinePoints = NULL;
1917 if (verbose || (parent && !header)) {
1918 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
1921 if (caoNbrLine > 100000) {
1927 caoLinePoints =
new unsigned int[2 * caoNbrLine];
1929 unsigned int index1, index2;
1932 int idFace = startIdFace;
1934 for (
unsigned int k = 0; k < caoNbrLine; k++) {
1942 std::string endLine =
"";
1943 if (safeGetline(fileId, endLine).good()) {
1944 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1946 std::string segmentName =
"";
1949 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1950 segmentName = mapOfParams[
"name"];
1952 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1953 minLineLengthThresh = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1955 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1959 SegmentInfo segmentInfo;
1960 segmentInfo.name = segmentName;
1961 segmentInfo.useLod = useLod;
1962 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1964 caoLinePoints[2 * k] = index1;
1965 caoLinePoints[2 * k + 1] = index2;
1967 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1968 std::vector<vpPoint> extremities;
1969 extremities.push_back(caoPoints[index1]);
1970 extremities.push_back(caoPoints[index2]);
1971 segmentInfo.extremities = extremities;
1973 std::pair<unsigned int, unsigned int> key(index1, index2);
1975 segmentTemporaryMap[key] = segmentInfo;
1977 vpTRACE(
" line %d has wrong coordinates.", k);
1989 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1990 unsigned int caoNbrPolygonLine;
1991 fileId >> caoNbrPolygonLine;
1992 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1995 if (verbose || (parent && !header)) {
1996 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon lines" << std::endl;
1999 if (caoNbrPolygonLine > 100000) {
2001 delete[] caoLinePoints;
2006 for (
unsigned int k = 0; k < caoNbrPolygonLine; k++) {
2009 unsigned int nbLinePol;
2010 fileId >> nbLinePol;
2011 std::vector<vpPoint> corners;
2012 if (nbLinePol > 100000) {
2016 for (
unsigned int n = 0; n < nbLinePol; n++) {
2019 if (index >= caoNbrLine) {
2022 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2023 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2025 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2026 faceSegmentKeyVector.push_back(key);
2031 std::string endLine =
"";
2032 if (safeGetline(fileId, endLine).good()) {
2033 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2035 std::string polygonName =
"";
2038 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2039 polygonName = mapOfParams[
"name"];
2041 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2042 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2044 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2057 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2058 it != segmentTemporaryMap.end(); ++it) {
2059 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2060 faceSegmentKeyVector.end()) {
2062 it->second.minLineLengthThresh);
2066 it->second.minLineLengthThresh);
2075 unsigned int caoNbrPolygonPoint;
2076 fileId >> caoNbrPolygonPoint;
2077 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2080 if (verbose || (parent && !header)) {
2081 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon points" << std::endl;
2084 if (caoNbrPolygonPoint > 100000) {
2088 for (
unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2091 unsigned int nbPointPol;
2092 fileId >> nbPointPol;
2093 if (nbPointPol > 100000) {
2096 std::vector<vpPoint> corners;
2097 for (
unsigned int n = 0; n < nbPointPol; n++) {
2099 if (index > caoNbrPoint - 1) {
2102 corners.push_back(caoPoints[index]);
2107 std::string endLine =
"";
2108 if (safeGetline(fileId, endLine).good()) {
2109 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2111 std::string polygonName =
"";
2114 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2115 polygonName = mapOfParams[
"name"];
2117 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2118 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2120 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2133 unsigned int caoNbCylinder;
2140 delete[] caoLinePoints;
2145 fileId >> caoNbCylinder;
2146 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2149 if (verbose || (parent && !header)) {
2150 std::cout <<
"> " << caoNbCylinder <<
" cylinders" << std::endl;
2153 if (caoNbCylinder > 100000) {
2157 for (
unsigned int k = 0; k < caoNbCylinder; ++k) {
2161 unsigned int indexP1, indexP2;
2168 std::string endLine =
"";
2169 if (safeGetline(fileId, endLine).good()) {
2170 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2172 std::string polygonName =
"";
2175 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2176 polygonName = mapOfParams[
"name"];
2178 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
2179 minLineLengthThreshold = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
2181 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2185 int idRevolutionAxis = idFace;
2186 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2188 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
2190 std::vector<std::vector<vpPoint> > listFaces;
2192 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2194 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2203 }
catch (
const std::exception& e) {
2204 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2205 std::cerr <<
"Exception: " << e.what() << std::endl;
2210 unsigned int caoNbCircle;
2217 delete[] caoLinePoints;
2222 fileId >> caoNbCircle;
2223 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2226 if (verbose || (parent && !header)) {
2227 std::cout <<
"> " << caoNbCircle <<
" circles" << std::endl;
2230 if (caoNbCircle > 100000) {
2234 for (
unsigned int k = 0; k < caoNbCircle; ++k) {
2238 unsigned int indexP1, indexP2, indexP3;
2246 std::string endLine =
"";
2247 if (safeGetline(fileId, endLine).good()) {
2248 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2250 std::string polygonName =
"";
2253 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2254 polygonName = mapOfParams[
"name"];
2256 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2257 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2259 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2263 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2264 minPolygonAreaThreshold);
2266 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2268 addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2269 minPolygonAreaThreshold);
2274 }
catch (
const std::exception& e) {
2275 std::cerr <<
"Cannot get the number of circles. Defaulting to zero." << std::endl;
2276 std::cerr <<
"Exception: " << e.what() << std::endl;
2280 startIdFace = idFace;
2283 delete[] caoLinePoints;
2285 if (header && parent) {
2287 std::cout <<
"Global information for " <<
vpIoTools::getName(modelFile) <<
" :" << std::endl;
2288 std::cout <<
"Total nb of points : " <<
nbPoints << std::endl;
2289 std::cout <<
"Total nb of lines : " <<
nbLines << std::endl;
2290 std::cout <<
"Total nb of polygon lines : " <<
nbPolygonLines << std::endl;
2291 std::cout <<
"Total nb of polygon points : " <<
nbPolygonPoints << std::endl;
2292 std::cout <<
"Total nb of cylinders : " <<
nbCylinders << std::endl;
2293 std::cout <<
"Total nb of circles : " <<
nbCircles << std::endl;
2295 std::cout <<
"> " <<
nbPoints <<
" points" << std::endl;
2296 std::cout <<
"> " <<
nbLines <<
" lines" << std::endl;
2297 std::cout <<
"> " <<
nbPolygonLines <<
" polygon lines" << std::endl;
2298 std::cout <<
"> " <<
nbPolygonPoints <<
" polygon points" << std::endl;
2299 std::cout <<
"> " <<
nbCylinders <<
" cylinders" << std::endl;
2300 std::cout <<
"> " <<
nbCircles <<
" circles" << std::endl;
2305 vectorOfModelFilename.pop_back();
2306 }
catch (
const std::exception& e) {
2307 std::cerr <<
"Cannot read line!" << std::endl;
2308 std::cerr <<
"Exception: " << e.what() << std::endl;
2313#ifdef VISP_HAVE_COIN3D
2324 SoVRMLTransform *sceneGraphVRML2Trasnform =
dynamic_cast<SoVRMLTransform *
>(sceneGraphVRML2);
2325 if (sceneGraphVRML2Trasnform) {
2326 float rx, ry, rz, rw;
2327 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2333 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2334 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2335 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2341 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2342 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2343 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2347 for (
unsigned int i = 0; i < 3; i++)
2349 for (
unsigned int i = 0; i < 3; i++)
2351 for (
unsigned int i = 0; i < 3; i++)
2355 transform = transform * transformCur;
2358 int nbShapes = sceneGraphVRML2->getNumChildren();
2365 for (
int i = 0; i < nbShapes; i++) {
2367 child = sceneGraphVRML2->getChild(i);
2369 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2370 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2373 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2374 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2377 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2378 SoChildList *child2list = child->getChildren();
2379 std::string name = child->getName().getString();
2381 for (
int j = 0; j < child2list->getLength(); j++) {
2382 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2383 SoVRMLIndexedFaceSet *face_set;
2384 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2385 if (!strncmp(face_set->getName().getString(),
"cyl", 3)) {
2391 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2392 SoVRMLIndexedLineSet *line_set;
2393 line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2411 const std::string &polygonName)
2413 std::vector<vpPoint> corners;
2417 int indexListSize = face_set->coordIndex.getNum();
2421 SoVRMLCoordinate *coord;
2423 for (
int i = 0; i < indexListSize; i++) {
2424 if (face_set->coordIndex[i] == -1) {
2425 if (corners.size() > 1) {
2434 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2435 int index = face_set->coordIndex[i];
2436 pointTransformed[0] = coord->point[index].getValue()[0];
2437 pointTransformed[1] = coord->point[index].getValue()[1];
2438 pointTransformed[2] = coord->point[index].getValue()[2];
2439 pointTransformed[3] = 1.0;
2441 pointTransformed = transform * pointTransformed;
2444 corners.push_back(pt);
2464 const std::string &polygonName)
2466 std::vector<vpPoint> corners_c1, corners_c2;
2469 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2471 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
2473 if (indexListSize % 2 == 1) {
2474 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
2477 corners_c1.resize(indexListSize / 2);
2478 corners_c2.resize(indexListSize / 2);
2484 for (
int i = 0; i < coords->point.getNum(); ++i) {
2485 pointTransformed[0] = coords->point[i].getValue()[0];
2486 pointTransformed[1] = coords->point[i].getValue()[1];
2487 pointTransformed[2] = coords->point[i].getValue()[2];
2488 pointTransformed[3] = 1.0;
2490 pointTransformed = transform * pointTransformed;
2494 if (i < (
int)corners_c1.size()) {
2495 corners_c1[(
unsigned int)i] = pt;
2497 corners_c2[(
unsigned int)i - corners_c1.size()] = pt;
2505 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
2506 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
2507 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
2508 double radius_c1 = sqrt(dist.
sumSquare());
2509 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
2510 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
2511 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
2512 double radius_c2 = sqrt(dist.
sumSquare());
2514 if (std::fabs(radius_c1 - radius_c2) >
2515 (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))) {
2516 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
2523 int idRevolutionAxis = idFace;
2528 std::vector<std::vector<vpPoint> > listFaces;
2532 initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2550 std::vector<vpPoint> corners;
2553 int indexListSize = line_set->coordIndex.getNum();
2555 SbVec3f point(0, 0, 0);
2557 SoVRMLCoordinate *coord;
2559 for (
int i = 0; i < indexListSize; i++) {
2560 if (line_set->coordIndex[i] == -1) {
2561 if (corners.size() > 1) {
2570 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2571 int index = line_set->coordIndex[i];
2572 point[0] = coord->point[index].getValue()[0];
2573 point[1] = coord->point[index].getValue()[1];
2574 point[2] = coord->point[index].getValue()[2];
2577 corners.push_back(pt);
2596 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
2604 for (
unsigned int i = 0; i < pts.size(); ++i) {
2606 oY += pts[i].get_oY();
2607 oZ += pts[i].get_oZ();
2626std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2630 std::vector<vpPolygon> polygonsTmp;
2631 std::vector<std::vector<vpPoint> > roisPtTmp;
2634 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2639 if ((useVisibility &&
faces.
getPolygon()[i]->isvisible) || !useVisibility) {
2640 std::vector<vpImagePoint> roiPts;
2648 if (roiPts.size() <= 2) {
2652 polygonsTmp.push_back(
vpPolygon(roiPts));
2654 std::vector<vpPoint> polyPts;
2662 roisPtTmp.push_back(polyPts);
2667 if (orderPolygons) {
2669 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2670 for (
unsigned int i = 0; i < polygonsTmp.size(); i++) {
2671 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2672 for (
unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2673 x_centroid += roisPtTmp[i][j].get_X();
2674 y_centroid += roisPtTmp[i][j].get_Y();
2675 z_centroid += roisPtTmp[i][j].get_Z();
2678 x_centroid /= roisPtTmp[i].size();
2679 y_centroid /= roisPtTmp[i].size();
2680 z_centroid /= roisPtTmp[i].size();
2682 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2683 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2687 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2689 polygonsTmp.resize(listOfPolygonFaces.size());
2690 roisPtTmp.resize(listOfPolygonFaces.size());
2693 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2695 polygonsTmp[cpt] = it->polygon;
2696 roisPtTmp[cpt] = it->faceCorners;
2699 pairOfPolygonFaces.first = polygonsTmp;
2700 pairOfPolygonFaces.second = roisPtTmp;
2702 pairOfPolygonFaces.first = polygonsTmp;
2703 pairOfPolygonFaces.second = roisPtTmp;
2706 return pairOfPolygonFaces;
2721#ifndef VISP_HAVE_OGRE
2723 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2724 "will be used. setOgreVisibilityTest() set to false."
2738 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. "
2739 "Far clipping won't be considered.");
2741 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping "
2742 "won't be considered.");
2746 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2749#ifdef VISP_HAVE_OGRE
2767 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2768 if (name.empty() ||
faces[i]->name == name) {
2769 faces[i]->setLod(useLod);
2785 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2786 if (name.empty() ||
faces[i]->name == name) {
2787 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2802 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2803 if (name.empty() ||
faces[i]->name == name) {
2804 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2817 vpTRACE(
"Near clipping value cannot be superior than far clipping value. "
2818 "Near clipping won't be considered.");
2820 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping "
2821 "won't be considered.");
2825 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2828#ifdef VISP_HAVE_OGRE
2844 for (
unsigned int i = 0; i <
faces.
size(); i++)
2858 if (isoJoIdentity_) {
2892 double &mu,
bool &reStartFromLastIncrement,
vpColVector *
const w,
2903 error = m_error_prev;
2904 if (w != NULL && m_w_prev != NULL) {
2907 reStartFromLastIncrement =
true;
2917 if (isoJoIdentity_) {
2925 vpMatrix LTLmuI = LTL + (LMA * mu);
2932 if (w != NULL && m_w_prev != NULL)
2954 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2962 if (w != NULL && m_w_prev != NULL)
2995 for (
unsigned int i = 0; i < 6; i++)
3019 for (
unsigned int i = 0; i < 6; i++) {
3021 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3032 std::vector<std::vector<vpPoint> > &listFaces)
3054 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3058 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3062 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon())
3092 std::vector<vpPoint> pointsFace;
3093 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3094 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3095 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3096 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3097 listFaces.push_back(pointsFace);
3100 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3101 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3102 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3103 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3104 listFaces.push_back(pointsFace);
3107 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3108 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3109 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3110 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3111 listFaces.push_back(pointsFace);
3114 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3115 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3116 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3117 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3118 listFaces.push_back(pointsFace);
3136 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3137 dz <= std::numeric_limits<double>::epsilon())
3144 bool useLod,
double minPolygonAreaThreshold,
3145 double minLineLengthThreshold)
3147 std::vector<vpPoint> corners_without_duplicates;
3148 corners_without_duplicates.push_back(corners[0]);
3149 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
3150 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3151 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3152 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3153 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3154 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3155 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3156 corners_without_duplicates.push_back(corners[i + 1]);
3161 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
3169 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3170 polygon.
addPoint(j, corners_without_duplicates[j]);
3186 int idFace,
const std::string &polygonName,
bool useLod,
3187 double minPolygonAreaThreshold)
3214 y[0] = plane.
getA() / norm_Y;
3215 y[1] = plane.
getB() / norm_Y;
3216 y[2] = plane.
getC() / norm_Y;
3219 for (
unsigned int i = 0; i < 3; i++) {
3235 for (
unsigned int i = 0; i < 4; i++) {
3238 w_p = wMc * cMc_90 * c_p;
3261 bool useLod,
double minLineLengthThreshold)
3293 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3296 for (
unsigned int i = 0; i < listFaces.size(); i++) {
3298 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
3299 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
3300 polygon.
addPoint(j, listFaces[i][j]);
3327 bool already_here =
false;
3334 already_here =
true;
3340 if (!already_here) {
3367 const std::string &name)
3369 bool already_here =
false;
3381 if (!already_here) {
3397 const std::string &name)
3399 bool already_here =
false;
3412 if (!already_here) {
3427 double radius,
int idFace,
const std::string &name)
3433 int idFace,
const std::string &name)
3442 for (
unsigned int i = 0; i < nbpt - 1; i++)
3452 for (
unsigned int i = 0; i < nbpt - 1; i++)
3481 unsigned int nbFeatures = 0;
3484 if (nbFeatures > 0) {
3485 return vpMath::deg(totalProjectionError / (
double)nbFeatures);
3517#ifdef VISP_HAVE_OGRE
3548 double totalProjectionError = 0.0;
3553 for (
size_t a = 0; a < l->
meline.size(); a++) {
3554 if (l->
meline[a] != NULL) {
3555 double lineNormGradient;
3556 unsigned int lineNbFeatures;
3560 totalProjectionError += lineNormGradient;
3561 nbFeatures += lineNbFeatures;
3572 double cylinderNormGradient = 0;
3573 unsigned int cylinderNbFeatures = 0;
3577 totalProjectionError += cylinderNormGradient;
3578 nbFeatures += cylinderNbFeatures;
3582 double cylinderNormGradient = 0;
3583 unsigned int cylinderNbFeatures = 0;
3587 totalProjectionError += cylinderNormGradient;
3588 nbFeatures += cylinderNbFeatures;
3597 double circleNormGradient = 0;
3598 unsigned int circleNbFeatures = 0;
3602 totalProjectionError += circleNormGradient;
3603 nbFeatures += circleNbFeatures;
3607 return totalProjectionError;
3612 bool changed =
false;
3617#ifdef VISP_HAVE_OGRE
3628 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
3629 if ((*it)->meline[a] != NULL) {
3630 delete (*it)->meline[a];
3631 (*it)->meline[a] = NULL;
3635 (*it)->meline.clear();
3636 (*it)->nbFeature.clear();
3637 (*it)->nbFeatureTotal = 0;
3642 if ((*it)->meline1 != NULL) {
3643 delete (*it)->meline1;
3644 (*it)->meline1 = NULL;
3646 if ((*it)->meline2 != NULL) {
3647 delete (*it)->meline2;
3648 (*it)->meline2 = NULL;
3651 (*it)->nbFeature = 0;
3652 (*it)->nbFeaturel1 = 0;
3653 (*it)->nbFeaturel2 = 0;
3657 if ((*it)->meEllipse != NULL) {
3658 delete (*it)->meEllipse;
3659 (*it)->meEllipse = NULL;
3661 (*it)->nbFeature = 0;
3667 const bool doNotTrack =
true;
3672 bool isvisible =
false;
3676 int index = *itindex;
3696 for (
size_t a = 0; a < l->
meline.size(); a++) {
3697 if (l->
meline[a] != NULL)
3699 if (a < l->nbFeature.size())
3712 bool isvisible =
false;
3746 bool isvisible =
false;
3781 std::cout <<
" *********** Parsing XML for ME projection error ************ " << std::endl;
3783 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 functionnalities 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 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 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))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited 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 double getSobelKernelX(double *filter, unsigned int size)
static double getSobelKernelY(double *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 emited by the vpMatrix class and its derivates
@ incorrectMatrixSizeError
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
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int size() const
std::vector< PolygonType * > & getPolygon()
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)
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)
vpAROgre * getOgreContext()
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
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
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 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 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)
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)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
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.
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.
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.
vpMbtPolygon & getPolygon()
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)
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 functionnalities 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)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
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 emited by the vpTracker class and its derivates.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)