46 #include <Simd/SimdLib.hpp>
48 #include <visp3/core/vpColVector.h>
49 #include <visp3/core/vpDisplay.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMatrix.h>
52 #include <visp3/core/vpPoint.h>
53 #include <visp3/vision/vpPose.h>
54 #ifdef VISP_HAVE_MODULE_GUI
55 #include <visp3/gui/vpDisplayGDI.h>
56 #include <visp3/gui/vpDisplayOpenCV.h>
57 #include <visp3/gui/vpDisplayX.h>
59 #include <visp3/core/vpCameraParameters.h>
60 #include <visp3/core/vpColor.h>
61 #include <visp3/core/vpException.h>
62 #include <visp3/core/vpIoTools.h>
63 #include <visp3/core/vpPixelMeterConversion.h>
64 #ifdef VISP_HAVE_MODULE_IO
65 #include <visp3/io/vpImageIo.h>
67 #include <visp3/core/vpCPUFeatures.h>
68 #include <visp3/core/vpIoTools.h>
69 #include <visp3/core/vpMatrixException.h>
70 #include <visp3/core/vpTrackingException.h>
71 #include <visp3/mbt/vpMbTracker.h>
73 #include <visp3/core/vpImageFilter.h>
74 #include <visp3/mbt/vpMbtXmlGenericParser.h>
76 #ifdef VISP_HAVE_COIN3D
78 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
79 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
80 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
81 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
82 #include <Inventor/VRMLnodes/SoVRMLShape.h>
83 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
84 #include <Inventor/actions/SoGetMatrixAction.h>
85 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
86 #include <Inventor/actions/SoSearchAction.h>
87 #include <Inventor/actions/SoToVRML2Action.h>
88 #include <Inventor/actions/SoWriteAction.h>
89 #include <Inventor/misc/SoChildList.h>
90 #include <Inventor/nodes/SoSeparator.h>
93 #ifndef DOXYGEN_SHOULD_SKIP_THIS
101 SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
103 std::vector<vpPoint> extremities;
106 double minLineLengthThresh;
113 struct PolygonFaceInfo {
114 PolygonFaceInfo(
double dist,
const vpPolygon &poly,
const std::vector<vpPoint> &corners)
115 : distanceToCamera(dist), polygon(poly), faceCorners(corners)
119 bool operator<(
const PolygonFaceInfo &pfi)
const {
return distanceToCamera < pfi.distanceToCamera; }
121 double distanceToCamera;
123 std::vector<vpPoint> faceCorners;
133 std::istream &safeGetline(std::istream &is, std::string &t)
143 std::istream::sentry se(is,
true);
144 std::streambuf *sb = is.rdbuf();
147 int c = sb->sbumpc();
150 }
else if (c ==
'\r') {
151 if (sb->sgetc() ==
'\n')
154 }
else if (c == std::streambuf::traits_type::eof()) {
157 is.setstate(std::ios::eofbit);
173 : m_cam(), m_cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
174 computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
175 displayFeatures(false), m_optimizationMethod(
vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(
vpMath::rad(89)),
176 angleDisappears(
vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(
vpPolygon3D::NO_CLIPPING),
177 useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
178 nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
179 minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
180 m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
181 m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(),
182 m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5),
183 m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20),
184 m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false),
224 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
232 #ifdef VISP_HAVE_MODULE_GUI
241 std::string ext =
".init";
242 std::string str_pose =
"";
243 size_t pos = initFile.rfind(ext);
246 std::fstream finitpos;
248 std::stringstream ss;
250 if (pos != std::string::npos)
251 str_pose = initFile.substr(0, pos) +
".0.pos";
253 str_pose = initFile +
".0.pos";
255 finitpos.open(str_pose.c_str(), std::ios::in);
261 if (finitpos.fail()) {
262 std::cout <<
"Cannot read " << ss.str() << std::endl <<
"cMo set to identity" << std::endl;
265 for (
unsigned int i = 0; i < 6; i += 1) {
266 finitpos >> init_pos[i];
272 std::cout <<
"last_cMo : " << std::endl << last_cMo << std::endl;
286 std::cout <<
"No modification : left click " << std::endl;
287 std::cout <<
"Modify initial pose : right click " << std::endl;
325 ss.str(std::string());
331 if (pos != std::string::npos) {
338 std::cout <<
"Load 3D points from: " << ss.str() << std::endl;
339 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
340 finit.open(ss.str());
342 finit.open(ss.str().c_str());
345 std::cout <<
"Cannot read " << ss.str() << std::endl;
349 #ifdef VISP_HAVE_MODULE_IO
353 const std::string imgExtVec[] = {
".ppm",
".pgm",
".jpg",
".jpeg",
".png"};
355 bool foundHelpImg =
false;
356 if (pos != std::string::npos) {
357 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
358 dispF = initFile.substr(0, pos) + imgExtVec[i];
362 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
363 dispF = initFile + imgExtVec[i];
369 std::cout <<
"Load image to help initialization: " << dispF << std::endl;
370 #if defined VISP_HAVE_X11
372 #elif defined VISP_HAVE_GDI
374 #elif defined VISP_HAVE_OPENCV
380 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
384 d_help->init(Iref, winXPos + (
int)width + 80, winYPos,
"Where to initialize...");
391 if (d_help != NULL) {
404 finit.ignore(256,
'\n');
405 std::cout <<
"Number of 3D points " << n3d << std::endl;
411 std::vector<vpPoint> P(n3d);
412 for (
unsigned int i = 0; i < n3d; i++) {
420 finit.ignore(256,
'\n');
423 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << pt_3d_tf[0] <<
" " << pt_3d_tf[1] <<
" "
424 << pt_3d_tf[2] << std::endl;
426 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
431 bool isWellInit =
false;
432 while (!isWellInit) {
433 std::vector<vpImagePoint> mem_ip;
434 for (
unsigned int i = 0; i < n3d; i++) {
435 std::ostringstream text;
436 text <<
"Click on point " << i + 1;
440 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
447 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
453 std::cout <<
"Click on point " << i + 1 <<
" ";
457 mem_ip.push_back(ip);
461 mem_ip.push_back(ip);
468 std::cout <<
"with 2D coordinates: " << ip << std::endl;
482 d1 = d2 = std::numeric_limits<double>::max();
551 if (d_help != NULL) {
557 std::cout <<
"cMo : " << std::endl <<
m_cMo << std::endl;
601 initClick(&I, NULL, initFile, displayHelp, T);
638 initClick(NULL, &I_color, initFile, displayHelp, T);
642 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile)
655 std::vector<vpPoint> P;
656 for (
unsigned int i = 0; i < points3D_list.size(); i++)
657 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
659 #ifdef VISP_HAVE_MODULE_IO
664 std::cout <<
"Load image to help initialization: " << displayFile << std::endl;
665 #if defined VISP_HAVE_X11
667 #elif defined VISP_HAVE_GDI
669 #elif defined VISP_HAVE_OPENCV
674 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
677 "Where to initialize...");
686 if (d_help != NULL) {
697 bool isWellInit =
false;
698 while (!isWellInit) {
699 for (
unsigned int i = 0; i < points3D_list.size(); i++) {
700 std::cout <<
"Click on point " << i + 1 << std::endl;
715 std::cout <<
"Click on point " << ip << std::endl;
732 d1 = d2 = std::numeric_limits<double>::max();
797 if (d_help != NULL) {
822 const std::string &displayFile)
824 initClick(&I, NULL, points3D_list, displayFile);
839 const std::string &displayFile)
841 initClick(NULL, &I_color, points3D_list, displayFile);
846 const std::string &initFile)
848 std::stringstream ss;
851 std::string ext =
".init";
852 size_t pos = initFile.rfind(ext);
854 if (pos == initFile.size() - ext.size() && pos != 0) {
861 std::cout <<
"Load 2D/3D points from: " << ss.str() << std::endl;
862 finit.open(ss.str().c_str(), std::ios::in);
864 std::cout <<
"cannot read " << ss.str() << std::endl;
874 while (!finit.fail() && (c ==
'#')) {
875 finit.ignore(256,
'\n');
882 finit.ignore(256,
'\n');
883 std::cout <<
"Number of 3D points " << n3d << std::endl;
890 for (
unsigned int i = 0; i < n3d; i++) {
893 while (!finit.fail() && (c ==
'#')) {
894 finit.ignore(256,
'\n');
902 finit.ignore(256,
'\n');
904 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << X <<
" " << Y <<
" " << Z << std::endl;
913 while (!finit.fail() && (c ==
'#')) {
914 finit.ignore(256,
'\n');
921 finit.ignore(256,
'\n');
922 std::cout <<
"Number of 2D points " << n2d << std::endl;
932 "In %s file, number of 2D points %d and number of 3D "
933 "points %d are not equal",
934 ss.str().c_str(), n2d, n3d);
938 for (
unsigned int i = 0; i < n2d; i++) {
941 while (!finit.fail() && (c ==
'#')) {
942 finit.ignore(256,
'\n');
946 double u, v, x = 0, y = 0;
949 finit.ignore(256,
'\n');
952 std::cout <<
"Point " << i + 1 <<
" with 2D coordinates: " << ip << std::endl;
963 d1 = d2 = std::numeric_limits<double>::max();
1053 const std::vector<vpImagePoint> &points2D_list,
1054 const std::vector<vpPoint> &points3D_list)
1056 if (points2D_list.size() != points3D_list.size())
1057 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points "
1058 "different to the number of 3D points.");
1060 size_t size = points3D_list.size();
1061 std::vector<vpPoint> P;
1064 for (
size_t i = 0; i < size; i++) {
1065 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1066 double x = 0, y = 0;
1075 d1 = d2 = std::numeric_limits<double>::max();
1113 const std::vector<vpPoint> &points3D_list)
1127 const std::vector<vpPoint> &points3D_list)
1133 const std::string &initFile)
1135 std::stringstream ss;
1139 std::string ext =
".pos";
1140 size_t pos = initFile.rfind(ext);
1142 if (pos == initFile.size() - ext.size() && pos != 0) {
1149 finit.open(ss.str().c_str(), std::ios::in);
1151 std::cout <<
"Cannot read " << ss.str() << std::endl;
1155 for (
unsigned int i = 0; i < 6; i += 1) {
1156 finit >> init_pos[i];
1273 std::fstream finitpos;
1274 finitpos.open(filename.c_str(), std::ios::out);
1277 finitpos << init_pos;
1282 bool useLod,
double minPolygonAreaThreshold,
double minLineLengthThreshold)
1284 std::vector<vpPoint> corners_without_duplicates;
1285 corners_without_duplicates.push_back(corners[0]);
1286 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
1287 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1288 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1289 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1290 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1291 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1292 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1293 corners_without_duplicates.push_back(corners[i + 1]);
1298 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
1320 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1321 polygon.
addPoint(j, corners_without_duplicates[j]);
1337 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold)
1370 y[0] = plane.
getA() / norm_Y;
1371 y[1] = plane.
getB() / norm_Y;
1372 y[2] = plane.
getC() / norm_Y;
1375 for (
unsigned int i = 0; i < 3; i++) {
1391 for (
unsigned int i = 0; i < 4; i++) {
1394 w_p = wMc * cMc_90 * c_p;
1417 bool useLod,
double minLineLengthThreshold)
1455 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
1458 for (
unsigned int i = 0; i < listFaces.size(); i++) {
1460 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
1461 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
1462 polygon.
addPoint(j, listFaces[i][j]);
1503 std::string::const_iterator it;
1506 it = modelFile.end();
1507 if ((*(it - 1) ==
'o' && *(it - 2) ==
'a' && *(it - 3) ==
'c' && *(it - 4) ==
'.') ||
1508 (*(it - 1) ==
'O' && *(it - 2) ==
'A' && *(it - 3) ==
'C' && *(it - 4) ==
'.')) {
1509 std::vector<std::string> vectorOfModelFilename;
1517 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose,
true, odTo);
1518 }
else if ((*(it - 1) ==
'l' && *(it - 2) ==
'r' && *(it - 3) ==
'w' && *(it - 4) ==
'.') ||
1519 (*(it - 1) ==
'L' && *(it - 2) ==
'R' && *(it - 3) ==
'W' && *(it - 4) ==
'.')) {
1552 #ifdef VISP_HAVE_COIN3D
1557 SbBool ok = in.openFile(modelFile.c_str());
1558 SoVRMLGroup *sceneGraphVRML2;
1565 if (!in.isFileVRML2()) {
1566 SoSeparator *sceneGraph = SoDB::readAll(&in);
1567 if (sceneGraph == NULL) {
1571 SoToVRML2Action tovrml2;
1572 tovrml2.apply(sceneGraph);
1574 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1575 sceneGraphVRML2->ref();
1576 sceneGraph->unref();
1578 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1579 if (sceneGraphVRML2 == NULL) {
1581 sceneGraphVRML2->ref();
1590 sceneGraphVRML2->unref();
1592 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1602 while (!fileId.fail() && (c ==
'#')) {
1603 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1606 if (fileId.fail()) {
1614 std::map<std::string, std::string> mapOfParams;
1617 while (!endLine.empty() && !exit) {
1623 std::string param(it->first +
"=");
1626 if (endLine.compare(0, param.size(), param) == 0) {
1628 endLine = endLine.substr(param.size());
1630 bool parseQuote =
false;
1631 if (it->second ==
"string") {
1633 if (endLine.size() > 2 && endLine[0] ==
'"') {
1635 endLine = endLine.substr(1);
1636 size_t pos = endLine.find_first_of(
'"');
1638 if (pos != std::string::npos) {
1639 mapOfParams[it->first] = endLine.substr(0, pos);
1640 endLine = endLine.substr(pos + 1);
1650 size_t pos1 = endLine.find_first_of(
' ');
1651 size_t pos2 = endLine.find_first_of(
'\t');
1652 size_t pos = pos1 < pos2 ? pos1 : pos2;
1654 mapOfParams[it->first] = endLine.substr(0, pos);
1655 endLine = endLine.substr(pos + 1);
1716 std::ifstream fileId;
1717 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1718 fileId.open(modelFile.c_str(), std::ifstream::in);
1719 if (fileId.fail()) {
1720 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
1725 std::cout <<
"Model file : " << modelFile << std::endl;
1727 vectorOfModelFilename.push_back(modelFile);
1740 fileId >> caoVersion;
1741 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1743 std::cout <<
"in vpMbTracker::loadCAOModel() -> Bad parameter header "
1744 "file : use V0, V1, ...";
1746 "header file : use V0, V1, ...");
1753 const std::string prefix_load =
"load";
1757 bool header =
false;
1758 while (c ==
'l' || c ==
'L') {
1759 getline(fileId, line);
1761 if (!line.compare(0, prefix_load.size(), prefix_load)) {
1763 std::string paramsStr = line.substr(5);
1765 paramsStr = paramsStr.substr(0, paramsStr.find_first_of(
")"));
1769 for (
size_t i = 0; i < params.size(); i++) {
1773 if (!params.empty()) {
1775 std::string headerPathRead = params[0];
1776 headerPathRead = headerPathRead.substr(1);
1777 headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of(
"\""));
1779 std::string headerPath = headerPathRead;
1794 for (
size_t i = 1; i < params.size(); i++) {
1795 std::string param = params[i];
1797 const std::string prefix =
"t=[";
1798 if (!param.compare(0, prefix.size(), prefix)) {
1799 param = param.substr(prefix.size());
1800 param = param.substr(0, param.find_first_of(
"]"));
1803 if (values.size() == 3) {
1804 t[0] = atof(values[0].c_str());
1805 t[1] = atof(values[1].c_str());
1806 t[2] = atof(values[2].c_str());
1811 const std::string prefix =
"tu=[";
1812 if (!param.compare(0, prefix.size(), prefix)) {
1813 param = param.substr(prefix.size());
1814 param = param.substr(0, param.find_first_of(
"]"));
1817 if (values.size() == 3) {
1818 for (
size_t j = 0; j < values.size(); j++) {
1819 std::string value = values[j];
1821 size_t unitPos = value.find(
"deg");
1822 if (unitPos != std::string::npos) {
1823 value = value.substr(0, unitPos);
1827 unitPos = value.find(
"rad");
1828 if (unitPos != std::string::npos) {
1829 value = value.substr(0, unitPos);
1831 tu[
static_cast<unsigned int>(j)] = !radian ?
vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1839 bool cyclic =
false;
1840 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1841 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1842 if (headerPath == *it) {
1850 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose,
false, odTo * oTo_local);
1855 std::cout <<
"WARNING Cyclic dependency detected with file " << headerPath <<
" declared in " << modelFile
1867 unsigned int caoNbrPoint;
1868 fileId >> caoNbrPoint;
1869 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1872 if (verbose || (parent && !header)) {
1873 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
1876 if (caoNbrPoint > 100000) {
1880 if (caoNbrPoint == 0 && !header) {
1888 for (
unsigned int k = 0; k < caoNbrPoint; k++) {
1896 if (caoVersion == 2) {
1901 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1911 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1912 unsigned int caoNbrLine;
1913 fileId >> caoNbrLine;
1914 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1917 unsigned int *caoLinePoints = NULL;
1918 if (verbose || (parent && !header)) {
1919 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
1922 if (caoNbrLine > 100000) {
1928 caoLinePoints =
new unsigned int[2 * caoNbrLine];
1930 unsigned int index1, index2;
1933 int idFace = startIdFace;
1935 for (
unsigned int k = 0; k < caoNbrLine; k++) {
1943 std::string endLine =
"";
1944 if (safeGetline(fileId, endLine).good()) {
1945 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1947 std::string segmentName =
"";
1950 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1951 segmentName = mapOfParams[
"name"];
1953 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1954 minLineLengthThresh = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1956 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1960 SegmentInfo segmentInfo;
1961 segmentInfo.name = segmentName;
1962 segmentInfo.useLod = useLod;
1963 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1965 caoLinePoints[2 * k] = index1;
1966 caoLinePoints[2 * k + 1] = index2;
1968 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1969 std::vector<vpPoint> extremities;
1970 extremities.push_back(caoPoints[index1]);
1971 extremities.push_back(caoPoints[index2]);
1972 segmentInfo.extremities = extremities;
1974 std::pair<unsigned int, unsigned int> key(index1, index2);
1976 segmentTemporaryMap[key] = segmentInfo;
1978 vpTRACE(
" line %d has wrong coordinates.", k);
1990 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1991 unsigned int caoNbrPolygonLine;
1992 fileId >> caoNbrPolygonLine;
1993 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1996 if (verbose || (parent && !header)) {
1997 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon lines" << std::endl;
2000 if (caoNbrPolygonLine > 100000) {
2002 delete[] caoLinePoints;
2007 for (
unsigned int k = 0; k < caoNbrPolygonLine; k++) {
2010 unsigned int nbLinePol;
2011 fileId >> nbLinePol;
2012 std::vector<vpPoint> corners;
2013 if (nbLinePol > 100000) {
2017 for (
unsigned int n = 0; n < nbLinePol; n++) {
2020 if (index >= caoNbrLine) {
2023 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2024 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2026 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2027 faceSegmentKeyVector.push_back(key);
2032 std::string endLine =
"";
2033 if (safeGetline(fileId, endLine).good()) {
2034 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2036 std::string polygonName =
"";
2039 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2040 polygonName = mapOfParams[
"name"];
2042 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2043 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2045 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2059 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2060 it != segmentTemporaryMap.end(); ++it) {
2061 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2062 faceSegmentKeyVector.end()) {
2064 it->second.minLineLengthThresh);
2077 unsigned int caoNbrPolygonPoint;
2078 fileId >> caoNbrPolygonPoint;
2079 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2082 if (verbose || (parent && !header)) {
2083 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon points" << std::endl;
2086 if (caoNbrPolygonPoint > 100000) {
2090 for (
unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2093 unsigned int nbPointPol;
2094 fileId >> nbPointPol;
2095 if (nbPointPol > 100000) {
2098 std::vector<vpPoint> corners;
2099 for (
unsigned int n = 0; n < nbPointPol; n++) {
2101 if (index > caoNbrPoint - 1) {
2104 corners.push_back(caoPoints[index]);
2109 std::string endLine =
"";
2110 if (safeGetline(fileId, endLine).good()) {
2111 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2113 std::string polygonName =
"";
2116 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2117 polygonName = mapOfParams[
"name"];
2119 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2120 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2122 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2136 unsigned int caoNbCylinder;
2143 delete[] caoLinePoints;
2148 fileId >> caoNbCylinder;
2149 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2152 if (verbose || (parent && !header)) {
2153 std::cout <<
"> " << caoNbCylinder <<
" cylinders" << std::endl;
2156 if (caoNbCylinder > 100000) {
2160 for (
unsigned int k = 0; k < caoNbCylinder; ++k) {
2164 unsigned int indexP1, indexP2;
2171 std::string endLine =
"";
2172 if (safeGetline(fileId, endLine).good()) {
2173 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2175 std::string polygonName =
"";
2178 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2179 polygonName = mapOfParams[
"name"];
2181 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
2182 minLineLengthThreshold = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
2184 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2188 int idRevolutionAxis = idFace;
2189 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2192 minLineLengthThreshold);
2194 std::vector<std::vector<vpPoint> > listFaces;
2196 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2198 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2207 }
catch (
const std::exception &e) {
2208 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2209 std::cerr <<
"Exception: " << e.what() << std::endl;
2214 unsigned int caoNbCircle;
2221 delete[] caoLinePoints;
2226 fileId >> caoNbCircle;
2227 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2230 if (verbose || (parent && !header)) {
2231 std::cout <<
"> " << caoNbCircle <<
" circles" << std::endl;
2234 if (caoNbCircle > 100000) {
2238 for (
unsigned int k = 0; k < caoNbCircle; ++k) {
2242 unsigned int indexP1, indexP2, indexP3;
2250 std::string endLine =
"";
2251 if (safeGetline(fileId, endLine).good()) {
2252 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2254 std::string polygonName =
"";
2257 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2258 polygonName = mapOfParams[
"name"];
2260 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2261 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2263 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2267 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2268 minPolygonAreaThreshold);
2270 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2273 polygonName, useLod, minPolygonAreaThreshold);
2279 }
catch (
const std::exception &e) {
2280 std::cerr <<
"Cannot get the number of circles. Defaulting to zero." << std::endl;
2281 std::cerr <<
"Exception: " << e.what() << std::endl;
2285 startIdFace = idFace;
2288 delete[] caoLinePoints;
2290 if (header && parent) {
2292 std::cout <<
"Global information for " <<
vpIoTools::getName(modelFile) <<
" :" << std::endl;
2293 std::cout <<
"Total nb of points : " <<
nbPoints << std::endl;
2294 std::cout <<
"Total nb of lines : " <<
nbLines << std::endl;
2295 std::cout <<
"Total nb of polygon lines : " <<
nbPolygonLines << std::endl;
2296 std::cout <<
"Total nb of polygon points : " <<
nbPolygonPoints << std::endl;
2297 std::cout <<
"Total nb of cylinders : " <<
nbCylinders << std::endl;
2298 std::cout <<
"Total nb of circles : " <<
nbCircles << std::endl;
2300 std::cout <<
"> " <<
nbPoints <<
" points" << std::endl;
2301 std::cout <<
"> " <<
nbLines <<
" lines" << std::endl;
2302 std::cout <<
"> " <<
nbPolygonLines <<
" polygon lines" << std::endl;
2303 std::cout <<
"> " <<
nbPolygonPoints <<
" polygon points" << std::endl;
2304 std::cout <<
"> " <<
nbCylinders <<
" cylinders" << std::endl;
2305 std::cout <<
"> " <<
nbCircles <<
" circles" << std::endl;
2310 vectorOfModelFilename.pop_back();
2311 }
catch (
const std::exception &e) {
2312 std::cerr <<
"Cannot read line!" << std::endl;
2313 std::cerr <<
"Exception: " << e.what() << std::endl;
2318 #ifdef VISP_HAVE_COIN3D
2329 SoVRMLTransform *sceneGraphVRML2Trasnform =
dynamic_cast<SoVRMLTransform *
>(sceneGraphVRML2);
2330 if (sceneGraphVRML2Trasnform) {
2331 float rx, ry, rz, rw;
2332 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2338 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2339 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2340 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2346 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2347 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2348 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2352 for (
unsigned int i = 0; i < 3; i++)
2354 for (
unsigned int i = 0; i < 3; i++)
2356 for (
unsigned int i = 0; i < 3; i++)
2360 transform = transform * transformCur;
2363 int nbShapes = sceneGraphVRML2->getNumChildren();
2370 for (
int i = 0; i < nbShapes; i++) {
2372 child = sceneGraphVRML2->getChild(i);
2374 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2375 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2378 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2379 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2382 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2383 SoChildList *child2list = child->getChildren();
2384 std::string name = child->getName().getString();
2386 for (
int j = 0; j < child2list->getLength(); j++) {
2387 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2388 SoVRMLIndexedFaceSet *face_set;
2389 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2390 if (!strncmp(face_set->getName().getString(),
"cyl", 3)) {
2396 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
2397 SoVRMLIndexedLineSet *line_set;
2398 line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
2416 const std::string &polygonName)
2418 std::vector<vpPoint> corners;
2422 int indexListSize = face_set->coordIndex.getNum();
2426 SoVRMLCoordinate *coord;
2428 for (
int i = 0; i < indexListSize; i++) {
2429 if (face_set->coordIndex[i] == -1) {
2430 if (corners.size() > 1) {
2439 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2440 int index = face_set->coordIndex[i];
2441 pointTransformed[0] = coord->point[index].getValue()[0];
2442 pointTransformed[1] = coord->point[index].getValue()[1];
2443 pointTransformed[2] = coord->point[index].getValue()[2];
2444 pointTransformed[3] = 1.0;
2446 pointTransformed = transform * pointTransformed;
2449 corners.push_back(pt);
2469 const std::string &polygonName)
2471 std::vector<vpPoint> corners_c1, corners_c2;
2474 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2476 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
2478 if (indexListSize % 2 == 1) {
2479 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
2482 corners_c1.resize(indexListSize / 2);
2483 corners_c2.resize(indexListSize / 2);
2489 for (
int i = 0; i < coords->point.getNum(); ++i) {
2490 pointTransformed[0] = coords->point[i].getValue()[0];
2491 pointTransformed[1] = coords->point[i].getValue()[1];
2492 pointTransformed[2] = coords->point[i].getValue()[2];
2493 pointTransformed[3] = 1.0;
2495 pointTransformed = transform * pointTransformed;
2499 if (i < (
int)corners_c1.size()) {
2500 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) {
2575 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2576 int index = line_set->coordIndex[i];
2577 point[0] = coord->point[index].getValue()[0];
2578 point[1] = coord->point[index].getValue()[1];
2579 point[2] = coord->point[index].getValue()[2];
2582 corners.push_back(pt);
2601 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
2609 for (
unsigned int i = 0; i < pts.size(); ++i) {
2611 oY += pts[i].get_oY();
2612 oZ += pts[i].get_oZ();
2631 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2635 std::vector<vpPolygon> polygonsTmp;
2636 std::vector<std::vector<vpPoint> > roisPtTmp;
2639 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2644 if ((useVisibility &&
faces.
getPolygon()[i]->isvisible) || !useVisibility) {
2645 std::vector<vpImagePoint> roiPts;
2653 if (roiPts.size() <= 2) {
2657 polygonsTmp.push_back(
vpPolygon(roiPts));
2659 std::vector<vpPoint> polyPts;
2667 roisPtTmp.push_back(polyPts);
2672 if (orderPolygons) {
2674 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2675 for (
unsigned int i = 0; i < polygonsTmp.size(); i++) {
2676 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2677 for (
unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2678 x_centroid += roisPtTmp[i][j].get_X();
2679 y_centroid += roisPtTmp[i][j].get_Y();
2680 z_centroid += roisPtTmp[i][j].get_Z();
2683 x_centroid /= roisPtTmp[i].size();
2684 y_centroid /= roisPtTmp[i].size();
2685 z_centroid /= roisPtTmp[i].size();
2687 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2688 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2692 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2694 polygonsTmp.resize(listOfPolygonFaces.size());
2695 roisPtTmp.resize(listOfPolygonFaces.size());
2698 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2700 polygonsTmp[cpt] = it->polygon;
2701 roisPtTmp[cpt] = it->faceCorners;
2704 pairOfPolygonFaces.first = polygonsTmp;
2705 pairOfPolygonFaces.second = roisPtTmp;
2707 pairOfPolygonFaces.first = polygonsTmp;
2708 pairOfPolygonFaces.second = roisPtTmp;
2711 return pairOfPolygonFaces;
2726 #ifndef VISP_HAVE_OGRE
2728 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2729 "will be used. setOgreVisibilityTest() set to false."
2743 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. "
2744 "Far clipping won't be considered.");
2746 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping "
2747 "won't be considered.");
2751 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2754 #ifdef VISP_HAVE_OGRE
2772 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2773 if (name.empty() ||
faces[i]->name == name) {
2774 faces[i]->setLod(useLod);
2790 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2791 if (name.empty() ||
faces[i]->name == name) {
2792 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2807 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2808 if (name.empty() ||
faces[i]->name == name) {
2809 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2822 vpTRACE(
"Near clipping value cannot be superior than far clipping value. "
2823 "Near clipping won't be considered.");
2825 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping "
2826 "won't be considered.");
2830 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2833 #ifdef VISP_HAVE_OGRE
2849 for (
unsigned int i = 0; i <
faces.
size(); i++)
2863 if (isoJoIdentity_) {
2897 double &mu,
bool &reStartFromLastIncrement,
vpColVector *
const w,
2908 error = m_error_prev;
2909 if (w != NULL && m_w_prev != NULL) {
2912 reStartFromLastIncrement =
true;
2922 if (isoJoIdentity_) {
2930 vpMatrix LTLmuI = LTL + (LMA * mu);
2937 if (w != NULL && m_w_prev != NULL)
2959 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2967 if (w != NULL && m_w_prev != NULL)
3000 for (
unsigned int i = 0; i < 6; i++)
3024 for (
unsigned int i = 0; i < 6; i++) {
3026 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3037 std::vector<std::vector<vpPoint> > &listFaces)
3059 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3063 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3067 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon())
3097 std::vector<vpPoint> pointsFace;
3098 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3099 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3100 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3101 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3102 listFaces.push_back(pointsFace);
3105 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3106 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3107 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3108 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3109 listFaces.push_back(pointsFace);
3112 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3113 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3114 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3115 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3116 listFaces.push_back(pointsFace);
3119 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3120 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3121 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3122 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3123 listFaces.push_back(pointsFace);
3141 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3142 dz <= std::numeric_limits<double>::epsilon())
3149 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold,
3150 double minLineLengthThreshold)
3152 std::vector<vpPoint> corners_without_duplicates;
3153 corners_without_duplicates.push_back(corners[0]);
3154 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
3155 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3156 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3157 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3158 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3159 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3160 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3161 corners_without_duplicates.push_back(corners[i + 1]);
3166 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
3174 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3175 polygon.
addPoint(j, corners_without_duplicates[j]);
3191 int idFace,
const std::string &polygonName,
bool useLod,
3192 double minPolygonAreaThreshold)
3219 y[0] = plane.
getA() / norm_Y;
3220 y[1] = plane.
getB() / norm_Y;
3221 y[2] = plane.
getC() / norm_Y;
3224 for (
unsigned int i = 0; i < 3; i++) {
3240 for (
unsigned int i = 0; i < 4; i++) {
3243 w_p = wMc * cMc_90 * c_p;
3266 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3298 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3301 for (
unsigned int i = 0; i < listFaces.size(); i++) {
3303 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
3304 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
3305 polygon.
addPoint(j, listFaces[i][j]);
3332 bool already_here =
false;
3339 already_here =
true;
3345 if (!already_here) {
3372 int idFace,
const std::string &name)
3374 bool already_here =
false;
3387 if (!already_here) {
3403 const std::string &name)
3405 bool already_here =
false;
3418 if (!already_here) {
3433 int idFace,
const std::string &name)
3439 const std::string &name)
3448 for (
unsigned int i = 0; i < nbpt - 1; i++)
3458 for (
unsigned int i = 0; i < nbpt - 1; i++)
3487 unsigned int nbFeatures = 0;
3490 if (nbFeatures > 0) {
3491 return vpMath::deg(totalProjectionError / (
double)nbFeatures);
3523 #ifdef VISP_HAVE_OGRE
3554 double totalProjectionError = 0.0;
3559 for (
size_t a = 0; a < l->
meline.size(); a++) {
3560 if (l->
meline[a] != NULL) {
3561 double lineNormGradient;
3562 unsigned int lineNbFeatures;
3566 totalProjectionError += lineNormGradient;
3567 nbFeatures += lineNbFeatures;
3578 double cylinderNormGradient = 0;
3579 unsigned int cylinderNbFeatures = 0;
3583 totalProjectionError += cylinderNormGradient;
3584 nbFeatures += cylinderNbFeatures;
3588 double cylinderNormGradient = 0;
3589 unsigned int cylinderNbFeatures = 0;
3593 totalProjectionError += cylinderNormGradient;
3594 nbFeatures += cylinderNbFeatures;
3603 double circleNormGradient = 0;
3604 unsigned int circleNbFeatures = 0;
3608 totalProjectionError += circleNormGradient;
3609 nbFeatures += circleNbFeatures;
3613 return totalProjectionError;
3618 bool changed =
false;
3624 #ifdef VISP_HAVE_OGRE
3638 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
3639 if ((*it)->meline[a] != NULL) {
3640 delete (*it)->meline[a];
3641 (*it)->meline[a] = NULL;
3645 (*it)->meline.clear();
3646 (*it)->nbFeature.clear();
3647 (*it)->nbFeatureTotal = 0;
3652 if ((*it)->meline1 != NULL) {
3653 delete (*it)->meline1;
3654 (*it)->meline1 = NULL;
3656 if ((*it)->meline2 != NULL) {
3657 delete (*it)->meline2;
3658 (*it)->meline2 = NULL;
3661 (*it)->nbFeature = 0;
3662 (*it)->nbFeaturel1 = 0;
3663 (*it)->nbFeaturel2 = 0;
3668 if ((*it)->meEllipse != NULL) {
3669 delete (*it)->meEllipse;
3670 (*it)->meEllipse = NULL;
3672 (*it)->nbFeature = 0;
3678 const bool doNotTrack =
true;
3683 bool isvisible =
false;
3687 int index = *itindex;
3707 for (
size_t a = 0; a < l->
meline.size(); a++) {
3708 if (l->
meline[a] != NULL)
3710 if (a < l->nbFeature.size())
3723 bool isvisible =
false;
3757 bool isvisible =
false;
3792 std::cout <<
" *********** Parsing XML for ME projection error ************ " << std::endl;
3794 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 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 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
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
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.
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 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)