46 #include <visp3/core/vpConfig.h>
47 #if defined(VISP_HAVE_SIMDLIB)
48 #include <Simd/SimdLib.h>
51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpDebug.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 #if defined(VISP_HAVE_THREADS)
102 #ifndef DOXYGEN_SHOULD_SKIP_THIS
105 #if defined(VISP_HAVE_THREADS)
106 std::mutex g_mutex_cout;
113 SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) { }
115 std::vector<vpPoint> extremities;
118 double minLineLengthThresh;
125 struct PolygonFaceInfo
127 PolygonFaceInfo(
double dist,
const vpPolygon &poly,
const std::vector<vpPoint> &corners)
128 : distanceToCamera(dist), polygon(poly), faceCorners(corners)
131 bool operator<(
const PolygonFaceInfo &pfi)
const {
return distanceToCamera < pfi.distanceToCamera; }
133 double distanceToCamera;
135 std::vector<vpPoint> faceCorners;
145 std::istream &safeGetline(std::istream &is, std::string &t)
155 std::istream::sentry se(is,
true);
156 std::streambuf *sb = is.rdbuf();
159 int c = sb->sbumpc();
163 else if (c ==
'\r') {
164 if (sb->sgetc() ==
'\n')
168 else if (c == std::streambuf::traits_type::eof()) {
171 is.setstate(std::ios::eofbit);
188 : m_cam(), m_cMo(), oJo(6, 6), m_isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
189 computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
190 displayFeatures(false), m_optimizationMethod(
vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(
vpMath::rad(89)),
191 angleDisappears(
vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(
vpPolygon3D::NO_CLIPPING),
192 useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
193 nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
194 minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
195 m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
196 m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(),
197 m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5),
198 m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20),
199 m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(nullptr), m_I(), m_sodb_init_called(false),
239 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
247 #ifdef VISP_HAVE_MODULE_GUI
256 std::string ext =
".init";
257 std::string str_pose =
"";
258 size_t pos = initFile.rfind(ext);
261 std::fstream finitpos;
263 std::stringstream ss;
265 if (pos != std::string::npos)
266 str_pose = initFile.substr(0, pos) +
".0.pos";
268 str_pose = initFile +
".0.pos";
270 finitpos.open(str_pose.c_str(), std::ios::in);
277 if (finitpos.fail()) {
278 std::cout <<
"Cannot read " << ss.str() << std::endl <<
"cMo set to identity" << std::endl;
282 for (
unsigned int i = 0; i < 6; i += 1) {
283 finitpos >> init_pos[i];
289 std::cout <<
"Tracker initial pose read from " << ss.str() <<
": " << std::endl << last_cMo << std::endl;
304 std::cout <<
"No modification : left click " << std::endl;
305 std::cout <<
"Modify initial pose : right click " << std::endl;
346 ss.str(std::string());
352 if (pos != std::string::npos) {
360 std::cout <<
"Load 3D points from: " << ss.str() << std::endl;
361 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
362 finit.open(ss.str());
364 finit.open(ss.str().c_str());
367 std::cout <<
"Cannot read " << ss.str() << std::endl;
371 #ifdef VISP_HAVE_MODULE_IO
375 const std::string imgExtVec[] = {
".ppm",
".pgm",
".jpg",
".jpeg",
".png" };
377 bool foundHelpImg =
false;
378 if (pos != std::string::npos) {
379 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
380 dispF = initFile.substr(0, pos) + imgExtVec[i];
385 for (
size_t i = 0; i < 5 && !foundHelpImg; i++) {
386 dispF = initFile + imgExtVec[i];
392 std::cout <<
"Load image to help initialization: " << dispF << std::endl;
393 #if defined(VISP_HAVE_X11)
394 d_help =
new vpDisplayX;
395 #elif defined(VISP_HAVE_GDI)
397 #elif defined(HAVE_OPENCV_HIGHGUI)
403 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
407 d_help->init(Iref, winXPos + (
int)width + 80, winYPos,
"Where to initialize...");
415 if (d_help !=
nullptr) {
428 finit.ignore(256,
'\n');
429 std::cout <<
"Number of 3D points " << n3d << std::endl;
435 std::vector<vpPoint> P(n3d);
436 for (
unsigned int i = 0; i < n3d; i++) {
444 finit.ignore(256,
'\n');
447 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << pt_3d_tf[0] <<
" " << pt_3d_tf[1] <<
" "
448 << pt_3d_tf[2] << std::endl;
450 P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
455 bool isWellInit =
false;
456 while (!isWellInit) {
457 std::vector<vpImagePoint> mem_ip;
458 for (
unsigned int i = 0; i < n3d; i++) {
459 std::ostringstream text;
460 text <<
"Click on point " << i + 1;
464 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
472 for (
unsigned int k = 0; k < mem_ip.size(); k++) {
478 std::cout <<
"Click on point " << i + 1 <<
" ";
482 mem_ip.push_back(ip);
487 mem_ip.push_back(ip);
494 std::cout <<
"with 2D coordinates: " << ip << std::endl;
560 if (d_help !=
nullptr) {
566 std::cout <<
"cMo : " << std::endl <<
m_cMo << std::endl;
607 void vpMbTracker::initClick(
const vpImage<unsigned char> &I,
const std::string &initFile,
bool displayHelp,
610 initClick(&I,
nullptr, initFile, displayHelp, T);
644 void vpMbTracker::initClick(
const vpImage<vpRGBa> &I_color,
const std::string &initFile,
bool displayHelp,
647 initClick(
nullptr, &I_color, initFile, displayHelp, T);
651 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile)
665 std::vector<vpPoint> P;
666 for (
unsigned int i = 0; i < points3D_list.size(); i++)
667 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
669 #ifdef VISP_HAVE_MODULE_IO
674 std::cout <<
"Load image to help initialization: " << displayFile << std::endl;
675 #if defined(VISP_HAVE_X11)
676 d_help =
new vpDisplayX;
677 #elif defined(VISP_HAVE_GDI)
679 #elif defined VISP_HAVE_OPENCV
684 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
687 "Where to initialize...");
698 if (d_help !=
nullptr) {
709 bool isWellInit =
false;
710 while (!isWellInit) {
711 for (
unsigned int i = 0; i < points3D_list.size(); i++) {
712 std::cout <<
"Click on point " << i + 1 << std::endl;
728 std::cout <<
"Click on point " << ip << std::endl;
795 if (d_help !=
nullptr) {
820 const std::string &displayFile)
822 initClick(&I,
nullptr, points3D_list, displayFile);
836 void vpMbTracker::initClick(
const vpImage<vpRGBa> &I_color,
const std::vector<vpPoint> &points3D_list,
837 const std::string &displayFile)
839 initClick(
nullptr, &I_color, points3D_list, displayFile);
844 const std::string &initFile)
846 std::stringstream ss;
849 std::string ext =
".init";
850 size_t pos = initFile.rfind(ext);
852 if (pos == initFile.size() - ext.size() && pos != 0) {
860 std::cout <<
"Load 2D/3D points from: " << ss.str() << std::endl;
861 finit.open(ss.str().c_str(), std::ios::in);
863 std::cout <<
"cannot read " << ss.str() << std::endl;
873 while (!finit.fail() && (c ==
'#')) {
874 finit.ignore(256,
'\n');
881 finit.ignore(256,
'\n');
882 std::cout <<
"Number of 3D points " << n3d << std::endl;
889 for (
unsigned int i = 0; i < n3d; i++) {
892 while (!finit.fail() && (c ==
'#')) {
893 finit.ignore(256,
'\n');
901 finit.ignore(256,
'\n');
903 std::cout <<
"Point " << i + 1 <<
" with 3D coordinates: " << X <<
" " << Y <<
" " << Z << std::endl;
912 while (!finit.fail() && (c ==
'#')) {
913 finit.ignore(256,
'\n');
920 finit.ignore(256,
'\n');
921 std::cout <<
"Number of 2D points " << n2d << std::endl;
931 "In %s file, number of 2D points %d and number of 3D "
932 "points %d are not equal",
933 ss.str().c_str(), n2d, n3d);
937 for (
unsigned int i = 0; i < n2d; i++) {
940 while (!finit.fail() && (c ==
'#')) {
941 finit.ignore(256,
'\n');
945 double u, v, x = 0, y = 0;
948 finit.ignore(256,
'\n');
951 std::cout <<
"Point " << i + 1 <<
" with 2D coordinates: " << ip << std::endl;
1032 const std::vector<vpImagePoint> &points2D_list,
1033 const std::vector<vpPoint> &points3D_list)
1035 if (points2D_list.size() != points3D_list.size())
1036 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points "
1037 "different to the number of 3D points.");
1039 size_t size = points3D_list.size();
1040 std::vector<vpPoint> P;
1043 for (
size_t i = 0; i < size; i++) {
1044 P.push_back(
vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
1045 double x = 0, y = 0;
1072 const std::vector<vpPoint> &points3D_list)
1086 const std::vector<vpPoint> &points3D_list)
1092 const std::string &initFile)
1094 std::stringstream ss;
1098 std::string ext =
".pos";
1099 size_t pos = initFile.rfind(ext);
1101 if (pos == initFile.size() - ext.size() && pos != 0) {
1109 finit.open(ss.str().c_str(), std::ios::in);
1111 std::cout <<
"Cannot read " << ss.str() << std::endl;
1115 for (
unsigned int i = 0; i < 6; i += 1) {
1116 finit >> init_pos[i];
1234 std::fstream finitpos;
1235 finitpos.open(filename.c_str(), std::ios::out);
1238 finitpos << init_pos;
1243 bool useLod,
double minPolygonAreaThreshold,
double minLineLengthThreshold)
1245 std::vector<vpPoint> corners_without_duplicates;
1246 corners_without_duplicates.push_back(corners[0]);
1247 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
1248 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
1249 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
1250 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
1251 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
1252 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
1253 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
1254 corners_without_duplicates.push_back(corners[i + 1]);
1259 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
1281 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
1282 polygon.
addPoint(j, corners_without_duplicates[j]);
1298 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold)
1331 y[0] = plane.
getA() / norm_Y;
1332 y[1] = plane.
getB() / norm_Y;
1333 y[2] = plane.
getC() / norm_Y;
1336 for (
unsigned int i = 0; i < 3; i++) {
1352 for (
unsigned int i = 0; i < 4; i++) {
1355 w_p = wMc * cMc_90 * c_p;
1378 bool useLod,
double minLineLengthThreshold)
1416 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
1419 for (
unsigned int i = 0; i < listFaces.size(); i++) {
1421 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
1422 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
1423 polygon.
addPoint(j, listFaces[i][j]);
1464 std::string::const_iterator it;
1467 it = modelFile.end();
1468 if ((*(it - 1) ==
'o' && *(it - 2) ==
'a' && *(it - 3) ==
'c' && *(it - 4) ==
'.') ||
1469 (*(it - 1) ==
'O' && *(it - 2) ==
'A' && *(it - 3) ==
'C' && *(it - 4) ==
'.')) {
1470 std::vector<std::string> vectorOfModelFilename;
1478 loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose,
true, odTo);
1480 else if ((*(it - 1) ==
'l' && *(it - 2) ==
'r' && *(it - 3) ==
'w' && *(it - 4) ==
'.') ||
1481 (*(it - 1) ==
'L' && *(it - 2) ==
'R' && *(it - 3) ==
'W' && *(it - 4) ==
'.')) {
1516 #ifdef VISP_HAVE_COIN3D
1521 SbBool ok = in.openFile(modelFile.c_str());
1522 SoVRMLGroup *sceneGraphVRML2;
1525 vpERROR_TRACE(
"can't open file to load model");
1529 if (!in.isFileVRML2()) {
1530 SoSeparator *sceneGraph = SoDB::readAll(&in);
1531 if (sceneGraph ==
nullptr) {
1535 SoToVRML2Action tovrml2;
1536 tovrml2.apply(sceneGraph);
1538 sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
1539 sceneGraphVRML2->ref();
1540 sceneGraph->unref();
1543 sceneGraphVRML2 = SoDB::readAllVRML(&in);
1544 if (sceneGraphVRML2 ==
nullptr) {
1546 sceneGraphVRML2->ref();
1555 sceneGraphVRML2->unref();
1557 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
1567 while (!fileId.fail() && (c ==
'#')) {
1568 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1571 if (fileId.fail()) {
1579 std::map<std::string, std::string> mapOfParams;
1582 while (!endLine.empty() && !exit) {
1588 std::string param(it->first +
"=");
1591 if (endLine.compare(0, param.size(), param) == 0) {
1593 endLine = endLine.substr(param.size());
1595 bool parseQuote =
false;
1596 if (it->second ==
"string") {
1598 if (endLine.size() > 2 && endLine[0] ==
'"') {
1600 endLine = endLine.substr(1);
1601 size_t pos = endLine.find_first_of(
'"');
1603 if (pos != std::string::npos) {
1604 mapOfParams[it->first] = endLine.substr(0, pos);
1605 endLine = endLine.substr(pos + 1);
1616 size_t pos1 = endLine.find_first_of(
' ');
1617 size_t pos2 = endLine.find_first_of(
'\t');
1618 size_t pos = pos1 < pos2 ? pos1 : pos2;
1620 mapOfParams[it->first] = endLine.substr(0, pos);
1621 endLine = endLine.substr(pos + 1);
1682 const unsigned int maxDataCAO = 100000;
1684 std::ifstream fileId;
1685 fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
1686 fileId.open(modelFile.c_str(), std::ifstream::in);
1687 if (fileId.fail()) {
1688 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
1693 std::cout <<
"Model file : " << modelFile << std::endl;
1695 vectorOfModelFilename.push_back(modelFile);
1708 fileId >> caoVersion;
1709 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1712 std::cout <<
"in vpMbTracker::loadCAOModel() -> Bad parameter header "
1713 "file : use V0, V1, ...";
1715 "header file : use V0, V1, ...");
1722 const std::string prefix_load =
"load";
1726 bool header =
false;
1727 while (c ==
'l' || c ==
'L') {
1728 getline(fileId, line);
1730 if (!line.compare(0, prefix_load.size(), prefix_load)) {
1732 std::string paramsStr = line.substr(5);
1734 paramsStr = paramsStr.substr(0, paramsStr.find_first_of(
")"));
1738 for (
size_t i = 0; i < params.size(); i++) {
1742 if (!params.empty()) {
1744 std::string headerPathRead = params[0];
1745 headerPathRead = headerPathRead.substr(1);
1746 headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of(
"\""));
1748 std::string headerPath = headerPathRead;
1763 for (
size_t i = 1; i < params.size(); i++) {
1764 std::string param = params[i];
1766 const std::string prefix =
"t=[";
1767 if (!param.compare(0, prefix.size(), prefix)) {
1768 param = param.substr(prefix.size());
1769 param = param.substr(0, param.find_first_of(
"]"));
1772 if (values.size() == 3) {
1773 t[0] = atof(values[0].c_str());
1774 t[1] = atof(values[1].c_str());
1775 t[2] = atof(values[2].c_str());
1780 const std::string prefix =
"tu=[";
1781 if (!param.compare(0, prefix.size(), prefix)) {
1782 param = param.substr(prefix.size());
1783 param = param.substr(0, param.find_first_of(
"]"));
1786 if (values.size() == 3) {
1787 for (
size_t j = 0; j < values.size(); j++) {
1788 std::string value = values[j];
1790 size_t unitPos = value.find(
"deg");
1791 if (unitPos != std::string::npos) {
1792 value = value.substr(0, unitPos);
1796 unitPos = value.find(
"rad");
1797 if (unitPos != std::string::npos) {
1798 value = value.substr(0, unitPos);
1800 tu[
static_cast<unsigned int>(j)] = !radian ?
vpMath::rad(atof(value.c_str())) : atof(value.c_str());
1808 bool cyclic =
false;
1809 for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
1810 it != vectorOfModelFilename.end() && !cyclic; ++it) {
1811 if (headerPath == *it) {
1819 loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose,
false, odTo * oTo_local);
1826 std::cout <<
"WARNING Cyclic dependency detected with file " << headerPath <<
" declared in " << modelFile
1838 unsigned int caoNbrPoint;
1839 fileId >> caoNbrPoint;
1840 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1843 if (verbose || (parent && !header)) {
1844 #if defined(VISP_HAVE_THREADS)
1845 std::lock_guard<std::mutex> lock(g_mutex_cout);
1847 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
1850 if (caoNbrPoint > maxDataCAO) {
1854 if (caoNbrPoint == 0 && !header) {
1857 std::vector<vpPoint> caoPoints(caoNbrPoint);
1859 for (
unsigned int k = 0; k < caoNbrPoint; k++) {
1867 if (caoVersion == 2) {
1874 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1877 caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
1884 std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
1885 unsigned int caoNbrLine;
1886 fileId >> caoNbrLine;
1887 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1890 std::vector<unsigned int> caoLinePoints;
1891 if (verbose || (parent && !header)) {
1892 #if defined(VISP_HAVE_THREADS)
1893 std::lock_guard<std::mutex> lock(g_mutex_cout);
1895 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
1898 if (caoNbrLine > maxDataCAO) {
1903 caoLinePoints.resize(2 * caoNbrLine);
1905 unsigned int index1, index2;
1908 int idFace = startIdFace;
1910 for (
unsigned int k = 0; k < caoNbrLine; k++) {
1918 std::string endLine =
"";
1919 if (safeGetline(fileId, endLine).good()) {
1920 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
1922 std::string segmentName =
"";
1925 if (mapOfParams.find(
"name") != mapOfParams.end()) {
1926 segmentName = mapOfParams[
"name"];
1928 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
1929 minLineLengthThresh = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
1931 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
1935 SegmentInfo segmentInfo;
1936 segmentInfo.name = segmentName;
1937 segmentInfo.useLod = useLod;
1938 segmentInfo.minLineLengthThresh = minLineLengthThresh;
1940 caoLinePoints[2 * k] = index1;
1941 caoLinePoints[2 * k + 1] = index2;
1943 if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
1944 std::vector<vpPoint> extremities;
1945 extremities.push_back(caoPoints[index1]);
1946 extremities.push_back(caoPoints[index2]);
1947 segmentInfo.extremities = extremities;
1949 std::pair<unsigned int, unsigned int> key(index1, index2);
1951 segmentTemporaryMap[key] = segmentInfo;
1954 vpTRACE(
" line %d has wrong coordinates.", k);
1966 std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
1967 unsigned int caoNbrPolygonLine;
1968 fileId >> caoNbrPolygonLine;
1969 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
1972 if (verbose || (parent && !header)) {
1973 #if defined(VISP_HAVE_THREADS)
1974 std::lock_guard<std::mutex> lock(g_mutex_cout);
1976 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon lines" << std::endl;
1979 if (caoNbrPolygonLine > maxDataCAO) {
1984 for (
unsigned int k = 0; k < caoNbrPolygonLine; k++) {
1987 unsigned int nbLinePol;
1988 fileId >> nbLinePol;
1989 std::vector<vpPoint> corners;
1990 if (nbLinePol > maxDataCAO) {
1994 for (
unsigned int n = 0; n < nbLinePol; n++) {
1997 if (index >= caoNbrLine) {
2000 corners.push_back(caoPoints[caoLinePoints[2 * index]]);
2001 corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
2003 std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
2004 faceSegmentKeyVector.push_back(key);
2009 std::string endLine =
"";
2010 if (safeGetline(fileId, endLine).good()) {
2011 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2013 std::string polygonName =
"";
2016 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2017 polygonName = mapOfParams[
"name"];
2019 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2020 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2022 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2036 for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
2037 it != segmentTemporaryMap.end(); ++it) {
2038 if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
2039 faceSegmentKeyVector.end()) {
2041 it->second.minLineLengthThresh);
2054 unsigned int caoNbrPolygonPoint;
2055 fileId >> caoNbrPolygonPoint;
2056 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2059 if (verbose || (parent && !header)) {
2060 #if defined(VISP_HAVE_THREADS)
2061 std::lock_guard<std::mutex> lock(g_mutex_cout);
2063 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon points" << std::endl;
2066 if (caoNbrPolygonPoint > maxDataCAO) {
2070 for (
unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
2073 unsigned int nbPointPol;
2074 fileId >> nbPointPol;
2075 if (nbPointPol > maxDataCAO) {
2078 std::vector<vpPoint> corners;
2079 for (
unsigned int n = 0; n < nbPointPol; n++) {
2081 if (index > caoNbrPoint - 1) {
2084 corners.push_back(caoPoints[index]);
2089 std::string endLine =
"";
2090 if (safeGetline(fileId, endLine).good()) {
2091 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2093 std::string polygonName =
"";
2096 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2097 polygonName = mapOfParams[
"name"];
2099 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2100 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2102 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2116 unsigned int caoNbCylinder;
2126 fileId >> caoNbCylinder;
2127 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2130 if (verbose || (parent && !header)) {
2131 #if defined(VISP_HAVE_THREADS)
2132 std::lock_guard<std::mutex> lock(g_mutex_cout);
2134 std::cout <<
"> " << caoNbCylinder <<
" cylinders" << std::endl;
2137 if (caoNbCylinder > maxDataCAO) {
2141 for (
unsigned int k = 0; k < caoNbCylinder; ++k) {
2145 unsigned int indexP1, indexP2;
2152 std::string endLine =
"";
2153 if (safeGetline(fileId, endLine).good()) {
2154 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2156 std::string polygonName =
"";
2159 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2160 polygonName = mapOfParams[
"name"];
2162 if (mapOfParams.find(
"minLineLengthThreshold") != mapOfParams.end()) {
2163 minLineLengthThreshold = std::atof(mapOfParams[
"minLineLengthThreshold"].c_str());
2165 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2169 int idRevolutionAxis = idFace;
2170 addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
2173 minLineLengthThreshold);
2175 std::vector<std::vector<vpPoint> > listFaces;
2177 addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
2179 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
2189 catch (
const std::exception &e) {
2190 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
2191 std::cerr <<
"Exception: " << e.what() << std::endl;
2196 unsigned int caoNbCircle;
2210 fileId.get(c_circle);
2211 int nb_circles = c_circle -
'0';
2212 if (nb_circles > 0) {
2216 fileId >> caoNbCircle;
2217 fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen(
'\n'));
2220 if (verbose || (parent && !header)) {
2221 #if defined(VISP_HAVE_THREADS)
2222 std::lock_guard<std::mutex> lock(g_mutex_cout);
2224 std::cout <<
"> " << caoNbCircle <<
" circles" << std::endl;
2227 if (caoNbCircle > maxDataCAO) {
2231 for (
unsigned int k = 0; k < caoNbCircle; ++k) {
2235 unsigned int indexP1, indexP2, indexP3;
2243 std::string endLine =
"";
2244 if (safeGetline(fileId, endLine).good()) {
2245 std::map<std::string, std::string> mapOfParams =
parseParameters(endLine);
2247 std::string polygonName =
"";
2250 if (mapOfParams.find(
"name") != mapOfParams.end()) {
2251 polygonName = mapOfParams[
"name"];
2253 if (mapOfParams.find(
"minPolygonAreaThreshold") != mapOfParams.end()) {
2254 minPolygonAreaThreshold = std::atof(mapOfParams[
"minPolygonAreaThreshold"].c_str());
2256 if (mapOfParams.find(
"useLod") != mapOfParams.end()) {
2260 addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
2261 minPolygonAreaThreshold);
2263 initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);
2266 polygonName, useLod, minPolygonAreaThreshold);
2273 catch (
const std::exception &e) {
2274 std::cerr <<
"Cannot get the number of circles. Defaulting to zero." << std::endl;
2275 std::cerr <<
"Exception: " << e.what() << std::endl;
2279 startIdFace = idFace;
2281 if (header && parent) {
2283 #if defined(VISP_HAVE_THREADS)
2284 std::lock_guard<std::mutex> lock(g_mutex_cout);
2286 std::cout <<
"Global information for " <<
vpIoTools::getName(modelFile) <<
" :" << std::endl;
2287 std::cout <<
"Total nb of points : " <<
nbPoints << std::endl;
2288 std::cout <<
"Total nb of lines : " <<
nbLines << std::endl;
2289 std::cout <<
"Total nb of polygon lines : " <<
nbPolygonLines << std::endl;
2290 std::cout <<
"Total nb of polygon points : " <<
nbPolygonPoints << std::endl;
2291 std::cout <<
"Total nb of cylinders : " <<
nbCylinders << std::endl;
2292 std::cout <<
"Total nb of circles : " <<
nbCircles << std::endl;
2295 #if defined(VISP_HAVE_THREADS)
2296 std::lock_guard<std::mutex> lock(g_mutex_cout);
2298 std::cout <<
"> " <<
nbPoints <<
" points" << std::endl;
2299 std::cout <<
"> " <<
nbLines <<
" lines" << std::endl;
2300 std::cout <<
"> " <<
nbPolygonLines <<
" polygon lines" << std::endl;
2301 std::cout <<
"> " <<
nbPolygonPoints <<
" polygon points" << std::endl;
2302 std::cout <<
"> " <<
nbCylinders <<
" cylinders" << std::endl;
2303 std::cout <<
"> " <<
nbCircles <<
" circles" << std::endl;
2308 vectorOfModelFilename.pop_back();
2310 catch (
const std::exception &e) {
2311 std::cerr <<
"Cannot read line!" << std::endl;
2312 std::cerr <<
"Exception: " << e.what() << std::endl;
2317 #ifdef VISP_HAVE_COIN3D
2328 SoVRMLTransform *sceneGraphVRML2Trasnform =
dynamic_cast<SoVRMLTransform *
>(sceneGraphVRML2);
2329 if (sceneGraphVRML2Trasnform) {
2330 float rx, ry, rz, rw;
2331 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
2337 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
2338 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
2339 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
2345 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
2346 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
2347 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
2351 for (
unsigned int i = 0; i < 3; i++)
2353 for (
unsigned int i = 0; i < 3; i++)
2355 for (
unsigned int i = 0; i < 3; i++)
2359 transform = transform * transformCur;
2362 int nbShapes = sceneGraphVRML2->getNumChildren();
2369 for (
int i = 0; i < nbShapes; i++) {
2371 child = sceneGraphVRML2->getChild(i);
2373 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
2374 extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
2377 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
2378 extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
2381 if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
2382 SoChildList *child2list = child->getChildren();
2383 std::string name = child->getName().getString();
2385 for (
int j = 0; j < child2list->getLength(); j++) {
2386 if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
2387 SoVRMLIndexedFaceSet *face_set;
2388 face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
2389 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) {
2440 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
2441 int index = face_set->coordIndex[i];
2442 pointTransformed[0] = coord->point[index].getValue()[0];
2443 pointTransformed[1] = coord->point[index].getValue()[1];
2444 pointTransformed[2] = coord->point[index].getValue()[2];
2445 pointTransformed[3] = 1.0;
2447 pointTransformed = transform * pointTransformed;
2450 corners.push_back(pt);
2470 const std::string &polygonName)
2472 std::vector<vpPoint> corners_c1, corners_c2;
2475 SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
2477 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
2479 if (indexListSize % 2 == 1) {
2480 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
2483 corners_c1.resize(indexListSize / 2);
2484 corners_c2.resize(indexListSize / 2);
2490 for (
int i = 0; i < coords->point.getNum(); ++i) {
2491 pointTransformed[0] = coords->point[i].getValue()[0];
2492 pointTransformed[1] = coords->point[i].getValue()[1];
2493 pointTransformed[2] = coords->point[i].getValue()[2];
2494 pointTransformed[3] = 1.0;
2496 pointTransformed = transform * pointTransformed;
2500 if (i < (
int)corners_c1.size()) {
2501 corners_c1[(
unsigned int)i] = pt;
2504 corners_c2[(
unsigned int)i - corners_c1.size()] = pt;
2512 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
2513 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
2514 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
2515 double radius_c1 = sqrt(dist.
sumSquare());
2516 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
2517 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
2518 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
2519 double radius_c2 = sqrt(dist.
sumSquare());
2521 if (std::fabs(radius_c1 - radius_c2) >
2522 (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))) {
2523 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
2530 int idRevolutionAxis = idFace;
2535 std::vector<std::vector<vpPoint> > listFaces;
2539 initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
2557 std::vector<vpPoint> corners;
2560 int indexListSize = line_set->coordIndex.getNum();
2562 SbVec3f point(0, 0, 0);
2564 SoVRMLCoordinate *coord;
2566 for (
int i = 0; i < indexListSize; i++) {
2567 if (line_set->coordIndex[i] == -1) {
2568 if (corners.size() > 1) {
2578 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
2579 int index = line_set->coordIndex[i];
2580 point[0] = coord->point[index].getValue()[0];
2581 point[1] = coord->point[index].getValue()[1];
2582 point[2] = coord->point[index].getValue()[2];
2585 corners.push_back(pt);
2604 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
2612 for (
unsigned int i = 0; i < pts.size(); ++i) {
2614 oY += pts[i].get_oY();
2615 oZ += pts[i].get_oZ();
2634 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
2638 std::vector<vpPolygon> polygonsTmp;
2639 std::vector<std::vector<vpPoint> > roisPtTmp;
2642 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
2647 if ((useVisibility &&
faces.
getPolygon()[i]->isvisible) || !useVisibility) {
2648 std::vector<vpImagePoint> roiPts;
2657 if (roiPts.size() <= 2) {
2661 polygonsTmp.push_back(
vpPolygon(roiPts));
2663 std::vector<vpPoint> polyPts;
2672 roisPtTmp.push_back(polyPts);
2677 if (orderPolygons) {
2679 std::vector<PolygonFaceInfo> listOfPolygonFaces;
2680 for (
unsigned int i = 0; i < polygonsTmp.size(); i++) {
2681 double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
2682 for (
unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
2683 x_centroid += roisPtTmp[i][j].get_X();
2684 y_centroid += roisPtTmp[i][j].get_Y();
2685 z_centroid += roisPtTmp[i][j].get_Z();
2688 x_centroid /= roisPtTmp[i].size();
2689 y_centroid /= roisPtTmp[i].size();
2690 z_centroid /= roisPtTmp[i].size();
2692 double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
2693 listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
2697 std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
2699 polygonsTmp.resize(listOfPolygonFaces.size());
2700 roisPtTmp.resize(listOfPolygonFaces.size());
2703 for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
2705 polygonsTmp[cpt] = it->polygon;
2706 roisPtTmp[cpt] = it->faceCorners;
2709 pairOfPolygonFaces.first = polygonsTmp;
2710 pairOfPolygonFaces.second = roisPtTmp;
2713 pairOfPolygonFaces.first = polygonsTmp;
2714 pairOfPolygonFaces.second = roisPtTmp;
2717 return pairOfPolygonFaces;
2732 #ifndef VISP_HAVE_OGRE
2734 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test "
2735 "will be used. setOgreVisibilityTest() set to false."
2749 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. "
2750 "Far clipping won't be considered.");
2752 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping "
2753 "won't be considered.");
2757 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2760 #ifdef VISP_HAVE_OGRE
2778 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2779 if (name.empty() ||
faces[i]->name == name) {
2780 faces[i]->setLod(useLod);
2796 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2797 if (name.empty() ||
faces[i]->name == name) {
2798 faces[i]->setMinLineLengthThresh(minLineLengthThresh);
2813 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2814 if (name.empty() ||
faces[i]->name == name) {
2815 faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
2828 vpTRACE(
"Near clipping value cannot be superior than far clipping value. "
2829 "Near clipping won't be considered.");
2831 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping "
2832 "won't be considered.");
2836 for (
unsigned int i = 0; i <
faces.
size(); i++) {
2839 #ifdef VISP_HAVE_OGRE
2855 for (
unsigned int i = 0; i <
faces.
size(); i++)
2869 if (isoJoIdentity) {
2897 #if defined(VISP_HAVE_SIMDLIB)
2900 const unsigned int N = interaction.
getRows();
2902 for (
unsigned int i = 0; i < 6; i += 1) {
2904 for (
unsigned int j = 0; j < N; j += 1) {
2905 ssum += interaction[j][i] * error[j];
2914 double &mu,
bool &reStartFromLastIncrement,
vpColVector *
const w,
2925 error = m_error_prev;
2926 if (w !=
nullptr && m_w_prev !=
nullptr) {
2929 reStartFromLastIncrement =
true;
2939 if (isoJoIdentity) {
2947 vpMatrix LTLmuI = LTL + (LMA * mu);
2954 if (w !=
nullptr && m_w_prev !=
nullptr)
2977 vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
2985 if (w !=
nullptr && m_w_prev !=
nullptr)
3018 for (
unsigned int i = 0; i < 6; i++)
3042 for (
unsigned int i = 0; i < 6; i++) {
3044 if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
3056 std::vector<std::vector<vpPoint> > &listFaces)
3078 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3082 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
3086 if (axisOrtho.
frobeniusNorm() < std::numeric_limits<double>::epsilon())
3116 std::vector<vpPoint> pointsFace;
3117 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3118 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3119 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3120 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3121 listFaces.push_back(pointsFace);
3124 pointsFace.push_back(
vpPoint(fc2[0], fc2[1], fc2[2]));
3125 pointsFace.push_back(
vpPoint(sc2[0], sc2[1], sc2[2]));
3126 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3127 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3128 listFaces.push_back(pointsFace);
3131 pointsFace.push_back(
vpPoint(fc3[0], fc3[1], fc3[2]));
3132 pointsFace.push_back(
vpPoint(sc3[0], sc3[1], sc3[2]));
3133 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3134 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3135 listFaces.push_back(pointsFace);
3138 pointsFace.push_back(
vpPoint(fc4[0], fc4[1], fc4[2]));
3139 pointsFace.push_back(
vpPoint(sc4[0], sc4[1], sc4[2]));
3140 pointsFace.push_back(
vpPoint(sc1[0], sc1[1], sc1[2]));
3141 pointsFace.push_back(
vpPoint(fc1[0], fc1[1], fc1[2]));
3142 listFaces.push_back(pointsFace);
3160 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
3161 dz <= std::numeric_limits<double>::epsilon())
3168 const std::string &polygonName,
bool useLod,
double minPolygonAreaThreshold,
3169 double minLineLengthThreshold)
3171 std::vector<vpPoint> corners_without_duplicates;
3172 corners_without_duplicates.push_back(corners[0]);
3173 for (
unsigned int i = 0; i < corners.size() - 1; i++) {
3174 if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
3175 std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
3176 std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
3177 std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
3178 std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
3179 std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
3180 corners_without_duplicates.push_back(corners[i + 1]);
3185 polygon.
setNbPoint((
unsigned int)corners_without_duplicates.size());
3193 for (
unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
3194 polygon.
addPoint(j, corners_without_duplicates[j]);
3210 int idFace,
const std::string &polygonName,
bool useLod,
3211 double minPolygonAreaThreshold)
3238 y[0] = plane.
getA() / norm_Y;
3239 y[1] = plane.
getB() / norm_Y;
3240 y[2] = plane.
getC() / norm_Y;
3243 for (
unsigned int i = 0; i < 3; i++) {
3259 for (
unsigned int i = 0; i < 4; i++) {
3262 w_p = wMc * cMc_90 * c_p;
3285 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3317 const std::string &polygonName,
bool useLod,
double minLineLengthThreshold)
3320 for (
unsigned int i = 0; i < listFaces.size(); i++) {
3322 polygon.
setNbPoint((
unsigned int)listFaces[i].size());
3323 for (
unsigned int j = 0; j < listFaces[i].size(); j++)
3324 polygon.
addPoint(j, listFaces[i][j]);
3351 bool already_here =
false;
3358 already_here =
true;
3364 if (!already_here) {
3391 int idFace,
const std::string &name)
3393 bool already_here =
false;
3406 if (!already_here) {
3422 const std::string &name)
3424 bool already_here =
false;
3437 if (!already_here) {
3452 int idFace,
const std::string &name)
3458 const std::string &name)
3467 for (
unsigned int i = 0; i < nbpt - 1; i++)
3477 for (
unsigned int i = 0; i < nbpt - 1; i++)
3506 unsigned int nbFeatures = 0;
3509 if (nbFeatures > 0) {
3510 return vpMath::deg(totalProjectionError / (
double)nbFeatures);
3542 #ifdef VISP_HAVE_OGRE
3573 double totalProjectionError = 0.0;
3578 for (
size_t a = 0; a < l->
meline.size(); a++) {
3579 if (l->
meline[a] !=
nullptr) {
3580 double lineNormGradient;
3581 unsigned int lineNbFeatures;
3585 totalProjectionError += lineNormGradient;
3586 nbFeatures += lineNbFeatures;
3597 double cylinderNormGradient = 0;
3598 unsigned int cylinderNbFeatures = 0;
3602 totalProjectionError += cylinderNormGradient;
3603 nbFeatures += cylinderNbFeatures;
3607 double cylinderNormGradient = 0;
3608 unsigned int cylinderNbFeatures = 0;
3612 totalProjectionError += cylinderNormGradient;
3613 nbFeatures += cylinderNbFeatures;
3622 double circleNormGradient = 0;
3623 unsigned int circleNbFeatures = 0;
3627 totalProjectionError += circleNormGradient;
3628 nbFeatures += circleNbFeatures;
3632 return totalProjectionError;
3637 bool changed =
false;
3644 #ifdef VISP_HAVE_OGRE
3658 for (
size_t a = 0; a < (*it)->meline.size(); a++) {
3659 if ((*it)->meline[a] !=
nullptr) {
3660 delete (*it)->meline[a];
3661 (*it)->meline[a] =
nullptr;
3665 (*it)->meline.clear();
3666 (*it)->nbFeature.clear();
3667 (*it)->nbFeatureTotal = 0;
3672 if ((*it)->meline1 !=
nullptr) {
3673 delete (*it)->meline1;
3674 (*it)->meline1 =
nullptr;
3676 if ((*it)->meline2 !=
nullptr) {
3677 delete (*it)->meline2;
3678 (*it)->meline2 =
nullptr;
3681 (*it)->nbFeature = 0;
3682 (*it)->nbFeaturel1 = 0;
3683 (*it)->nbFeaturel2 = 0;
3688 if ((*it)->meEllipse !=
nullptr) {
3689 delete (*it)->meEllipse;
3690 (*it)->meEllipse =
nullptr;
3692 (*it)->nbFeature = 0;
3698 const bool doNotTrack =
true;
3703 bool isvisible =
false;
3707 int index = *itindex;
3728 for (
size_t a = 0; a < l->
meline.size(); a++) {
3729 if (l->
meline[a] !=
nullptr)
3731 if (a < l->nbFeature.size())
3744 bool isvisible =
false;
3779 bool isvisible =
false;
3808 #if defined(VISP_HAVE_PUGIXML)
3816 std::cout <<
" *********** Parsing XML for ME projection error ************ " << std::endl;
3818 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...
Class that defines generic functionalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
int getWindowXPosition() const
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
int getWindowYPosition() const
static void flush(const vpImage< unsigned char > &I)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static FilterType getSobelKernelX(FilterType *filter, unsigned int size)
static FilterType getSobelKernelY(FilterType *filter, unsigned int size)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
static double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static double deg(double rad)
error that can be emitted by the vpMatrix class and its derivatives
@ incorrectMatrixSizeError
Incorrect matrix size.
Implementation of a matrix and operations on matrices.
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int size() const
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool isVisible(unsigned int i)
void addPolygon(PolygonType *p)
std::vector< PolygonType * > & getPolygon()
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void setOgreShowConfigDialog(bool showConfigDialog)
Main methods for a model-based tracker.
std::map< std::string, std::string > parseParameters(std::string &endLine)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
double m_lambda
Gain of the virtual visual servoing stage.
virtual vpColVector getEstimatedDoF() const
double computeProjectionErrorImpl(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam, unsigned int &nbFeatures)
virtual void setEstimatedDoF(const vpColVector &v)
void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
void removeComment(std::ifstream &fileId)
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
void projectionErrorResetMovingEdges()
void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
@ LEVENBERG_MARQUARDT_OPT
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="")
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
virtual void loadCAOModel(const std::string &modelFile, std::vector< std::string > &vectorOfModelFilename, int &startIdFace, bool verbose=false, bool parent=true, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void init(const vpImage< unsigned char > &I)=0
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool useLodGeneral
True if LOD mode is enabled.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
std::map< std::string, std::string > mapOfParameterNames
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void loadVRMLModel(const std::string &modelFile)
unsigned int nbLines
Number of lines in CAO model.
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void savePose(const std::string &filename) const
void addPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, double minLineLengthThreshold=50.0)
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
vpMatrix covarianceMatrix
Covariance matrix.
vpHomogeneousMatrix m_cMo
The current pose.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
vpMatrix m_SobelX
Sobel kernel in X.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
unsigned int nbPoints
Number of points in CAO model.
vpCameraParameters m_cam
The camera parameters.
std::string modelFileName
bool useOgre
Use Ogre3d for visibility tests.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
void projectionErrorInitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
virtual void setOgreVisibilityTest(const bool &v)
std::string poseSavingFilename
void setProjectionErrorKernelSize(const unsigned int &size)
void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
virtual void setLod(bool useLod, const std::string &name="")
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
virtual void computeVVSPoseEstimation(const bool isoJoIdentity, unsigned int iter, vpMatrix &L, vpMatrix <L, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, const vpColVector *const w=nullptr, vpColVector *const m_w_prev=nullptr)
double angleDisappears
Angle used to detect a face disappearance.
virtual void setNearClippingDistance(const double &dist)
void setProjectionErrorMovingEdge(const vpMe &me)
bool applyLodSettingInConfig
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
bool useScanLine
Use Scanline for visibility tests.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
vpMatrix m_SobelY
Sobel kernel in Y.
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
bool m_projectionErrorOgreShowConfigDialog
void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon)
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void addProjectionErrorPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon)
virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName="")
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
double distNearClip
Distance for near clipping.
bool m_sodb_init_called
Flag that indicates that SoDB::init(); was called.
void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
unsigned int nbCylinders
Number of cylinders in CAO model.
unsigned int clippingFlag
Flags specifying which clipping to used.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
unsigned int nbCircles
Number of circles in CAO model.
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts) const
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector< std::vector< vpPoint > > &listFaces)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setVisible(bool _isvisible)
void setMovingEdge(vpMe *Me)
void setCameraParameters(const vpCameraParameters &camera)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
void setIndex(unsigned int i)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the circle.
int index_polygon
Index of the faces which contain the line.
vpPoint * p3
An other point on the plane containing the circle.
vpMbtMeEllipse * meEllipse
The moving edge containers.
void setName(const std::string &circle_name)
Manage a cylinder used in the model-based tracker.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
unsigned int nbFeaturel2
The number of moving edges on line 2.
vpPoint * p2
The second extremity on the axe.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setMovingEdge(vpMe *Me)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
unsigned int nbFeature
The number of moving edges.
int index_polygon
Index of the face which contains the cylinder.
void setIndex(unsigned int i)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void setIndex(unsigned int i)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
vpMbtPolygon & getPolygon()
Implementation of a polygon of the model used by the model-based tracker.
void setMinPolygonAreaThresh(double min_polygon_area)
std::string getName() const
void setName(const std::string &face_name)
void setLod(bool use_lod)
virtual void setIndex(int i)
void setMinLineLengthThresh(double min_line_length)
void setIsPolygonOriented(const bool &oriented)
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void setProjectionErrorMe(const vpMe &me)
unsigned int getProjectionErrorKernelSize() const
void setProjectionErrorKernelSize(const unsigned int &size)
@ PROJECTION_ERROR_PARSER
void parse(const std::string &filename)
void getProjectionErrorMe(vpMe &me) const
void setVerbose(bool verbose)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
void set_x(double x)
Set the point x coordinate in the image plane.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_oY() const
Get the point oY coordinate in the object frame.
void setWorldCoordinates(double oX, double oY, double oZ)
void set_y(double y)
Set the point y coordinate in the image plane.
Implements a 3D polygon with render functionalities like clipping.
void setFarClippingDistance(const double &dist)
unsigned int getNbPoint() const
void setNearClippingDistance(const double &dist)
vpPoint * p
corners in the object frame
virtual void setNbPoint(unsigned int nb)
void setClipping(const unsigned int &flags)
void addPoint(unsigned int n, const vpPoint &P)
Defines a generic 2D polygon.
Implementation of a pose vector and operations on poses.
vpPoseVector & buildFrom(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void addPoint(const vpPoint &P)
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
Implementation of a rotation vector as quaternion angle minimal representation.
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)