diff --git a/modules/tracker/mbt/src/vpMbTracker.cpp b/modules/tracker/mbt/src/vpMbTracker.cpp index 68b7132512..304485f943 100644 --- a/modules/tracker/mbt/src/vpMbTracker.cpp +++ b/modules/tracker/mbt/src/vpMbTracker.cpp @@ -99,13 +99,14 @@ namespace { #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - std::mutex g_mutex_cout; +std::mutex g_mutex_cout; #endif /*! Structure to store info about segment in CAO model files. */ -struct SegmentInfo { - SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {} +struct SegmentInfo +{ + SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) { } std::vector extremities; std::string name; @@ -117,11 +118,11 @@ struct SegmentInfo { Structure to store info about a polygon face represented by a vpPolygon and by a list of vpPoint representing the corners of the polygon face in 3D. */ -struct PolygonFaceInfo { +struct PolygonFaceInfo +{ PolygonFaceInfo(double dist, const vpPolygon &poly, const std::vector &corners) : distanceToCamera(dist), polygon(poly), faceCorners(corners) - { - } + { } bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; } @@ -154,16 +155,19 @@ std::istream &safeGetline(std::istream &is, std::string &t) int c = sb->sbumpc(); if (c == '\n') { return is; - } else if (c == '\r') { + } + else if (c == '\r') { if (sb->sgetc() == '\n') sb->sbumpc(); return is; - } else if (c == std::streambuf::traits_type::eof()) { - // Also handle the case when the last line has no line ending + } + else if (c == std::streambuf::traits_type::eof()) { + // Also handle the case when the last line has no line ending if (t.empty()) is.setstate(std::ios::eofbit); return is; - } else { // default case + } + else { // default case t += (char)c; } } @@ -178,18 +182,18 @@ std::istream &safeGetline(std::istream &is, std::string &t) */ vpMbTracker::vpMbTracker() : m_cam(), m_cMo(), oJo(6, 6), m_isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(), - computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0), - displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)), - angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING), - useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0), - nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false), - minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(), - m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01), - m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(), - m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5), - m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), - m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false), - m_rand() + computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0), + displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)), + angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING), + useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0), + nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false), + minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(), + m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01), + m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(), m_projectionErrorFaces(), + m_projectionErrorOgreShowConfigDialog(false), m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5, 5), + m_SobelY(5, 5), m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), + m_projectionErrorDisplayThickness(1), m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false), + m_rand() { oJo.eye(); // Map used to parse additional information in CAO model files, @@ -261,14 +265,16 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage finitpos.open(str_pose.c_str(), std::ios::in); ss << str_pose; - } else { + } + else { finitpos.open(poseSavingFilename.c_str(), std::ios::in); ss << poseSavingFilename; } if (finitpos.fail()) { std::cout << "Cannot read " << ss.str() << std::endl << "cMo set to identity" << std::endl; last_cMo.eye(); - } else { + } + else { for (unsigned int i = 0; i < 6; i += 1) { finitpos >> init_pos[i]; } @@ -283,7 +289,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage display(*I, last_cMo, m_cam, vpColor::green, 1, true); vpDisplay::displayFrame(*I, last_cMo, m_cam, 0.05, vpColor::green); vpDisplay::flush(*I); - } else { + } + else { vpDisplay::display(*I_color); display(*I_color, last_cMo, m_cam, vpColor::green, 1, true); vpDisplay::displayFrame(*I_color, last_cMo, m_cam, 0.05, vpColor::green); @@ -300,7 +307,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage while (!vpDisplay::getClick(*I, ip, button)) { } - } else { + } + else { vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red); @@ -313,13 +321,15 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (!finitpos.fail() && button == vpMouseButton::button1) { m_cMo = last_cMo; - } else { + } + else { vpDisplay *d_help = NULL; if (I) { vpDisplay::display(*I); vpDisplay::flush(*I); - } else { + } + else { vpDisplay::display(*I_color); vpDisplay::flush(*I_color); } @@ -337,7 +347,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage // X Y Z if (pos != std::string::npos) { ss << initFile; - } else { + } + else { ss << initFile; ss << ".init"; } @@ -357,7 +368,7 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage // Display window creation and initialisation try { if (displayHelp) { - const std::string imgExtVec[] = {".ppm", ".pgm", ".jpg", ".jpeg", ".png"}; + const std::string imgExtVec[] = { ".ppm", ".pgm", ".jpg", ".jpeg", ".png" }; std::string dispF; bool foundHelpImg = false; if (pos != std::string::npos) { @@ -365,7 +376,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage dispF = initFile.substr(0, pos) + imgExtVec[i]; foundHelpImg = vpIoTools::checkFilename(dispF); } - } else { + } + else { for (size_t i = 0; i < 5 && !foundHelpImg; i++) { dispF = initFile + imgExtVec[i]; foundHelpImg = vpIoTools::checkFilename(dispF); @@ -394,7 +406,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage #endif } } - } catch (...) { + } + catch (...) { if (d_help != NULL) { delete d_help; d_help = NULL; @@ -428,7 +441,7 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage vpColVector pt_3d_tf = T * pt_3d; std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " " - << pt_3d_tf[2] << std::endl; + << pt_3d_tf[2] << std::endl; P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z) } @@ -448,7 +461,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage vpDisplay::displayCross(*I, mem_ip[k], 10, vpColor::green, 2); } vpDisplay::flush(*I); - } else { + } + else { vpDisplay::display(*I_color); vpDisplay::displayText(*I_color, 15, 10, text.str(), vpColor::red); for (unsigned int k = 0; k < mem_ip.size(); k++) { @@ -463,7 +477,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage vpDisplay::getClick(*I, ip); mem_ip.push_back(ip); vpDisplay::flush(*I); - } else { + } + else { vpDisplay::getClick(*I_color, ip); mem_ip.push_back(ip); vpDisplay::flush(*I_color); @@ -479,7 +494,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (I) { vpDisplay::flush(*I); vpDisplay::display(*I); - } else { + } + else { vpDisplay::flush(*I_color); vpDisplay::display(*I_color); } @@ -498,12 +514,14 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (button == vpMouseButton::button1) { isWellInit = true; - } else { + } + else { pose.clearPoint(); vpDisplay::display(*I); vpDisplay::flush(*I); } - } else { + } + else { display(*I_color, m_cMo, m_cam, vpColor::green, 1, true); vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red); @@ -516,7 +534,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (button == vpMouseButton::button1) { isWellInit = true; - } else { + } + else { pose.clearPoint(); vpDisplay::display(*I_color); vpDisplay::flush(*I_color); @@ -630,7 +649,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (I) { vpDisplay::display(*I); vpDisplay::flush(*I); - } else { + } + else { vpDisplay::display(*I_color); vpDisplay::flush(*I_color); } @@ -661,14 +681,16 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (I) { d_help->init(Iref, I->display->getWindowXPosition() + (int)I->getWidth() + 80, I->display->getWindowYPosition(), "Where to initialize..."); - } else { + } + else { d_help->init(Iref, I_color->display->getWindowXPosition() + (int)I_color->getWidth() + 80, I_color->display->getWindowYPosition(), "Where to initialize..."); } vpDisplay::display(Iref); vpDisplay::flush(Iref); #endif - } catch (...) { + } + catch (...) { if (d_help != NULL) { delete d_help; d_help = NULL; @@ -689,7 +711,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage vpDisplay::getClick(*I, ip); vpDisplay::displayCross(*I, ip, 5, vpColor::green); vpDisplay::flush(*I); - } else { + } + else { vpDisplay::getClick(*I_color, ip); vpDisplay::displayCross(*I_color, ip, 5, vpColor::green); vpDisplay::flush(*I_color); @@ -702,14 +725,16 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (I) { vpDisplay::displayPoint(*I, ip, vpColor::green); // display target point - } else { + } + else { vpDisplay::displayPoint(*I_color, ip, vpColor::green); // display target point } pose.addPoint(P[i]); // and added to the pose computation point list } if (I) { vpDisplay::flush(*I); - } else { + } + else { vpDisplay::flush(*I_color); } @@ -727,12 +752,14 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (button == vpMouseButton::button1) { isWellInit = true; - } else { + } + else { pose.clearPoint(); vpDisplay::display(*I); vpDisplay::flush(*I); } - } else { + } + else { display(*I_color, m_cMo, m_cam, vpColor::green, 1, true); vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red); @@ -745,7 +772,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (button == vpMouseButton::button1) { isWellInit = true; - } else { + } + else { pose.clearPoint(); vpDisplay::display(*I_color); vpDisplay::flush(*I_color); @@ -755,7 +783,8 @@ void vpMbTracker::initClick(const vpImage *const I, const vpImage if (I) { vpDisplay::displayFrame(*I, m_cMo, m_cam, 0.05, vpColor::red); - } else { + } + else { vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red); } @@ -818,7 +847,8 @@ void vpMbTracker::initFromPoints(const vpImage *const I, const vp if (pos == initFile.size() - ext.size() && pos != 0) { ss << initFile; - } else { + } + else { ss << initFile; ss << ".init"; } @@ -929,7 +959,8 @@ void vpMbTracker::initFromPoints(const vpImage *const I, const vp if (I) { init(*I); - } else { + } + else { vpImageConvert::convert(*I_color, m_I); init(m_I); } @@ -1018,7 +1049,8 @@ void vpMbTracker::initFromPoints(const vpImage *const I, const vp if (I) { init(*I); - } else { + } + else { vpImageConvert::convert(*I_color, m_I); init(m_I); } @@ -1064,7 +1096,8 @@ void vpMbTracker::initFromPose(const vpImage *const I, const vpIm if (pos == initFile.size() - ext.size() && pos != 0) { ss << initFile; - } else { + } + else { ss << initFile; ss << ".pos"; } @@ -1083,7 +1116,8 @@ void vpMbTracker::initFromPose(const vpImage *const I, const vpIm if (I) { init(*I); - } else { + } + else { vpImageConvert::convert(*I_color, m_I); init(m_I); } @@ -1438,13 +1472,16 @@ void vpMbTracker::loadModel(const std::string &modelFile, bool verbose, const vp nbCylinders = 0; nbCircles = 0; loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, odTo); - } else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') || - (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) { + } + else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') || + (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) { loadVRMLModel(modelFile); - } else { + } + else { throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str()); } - } else { + } + else { throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str()); } @@ -1497,7 +1534,8 @@ void vpMbTracker::loadVRMLModel(const std::string &modelFile) sceneGraphVRML2 = tovrml2.getVRML2SceneGraph(); sceneGraphVRML2->ref(); sceneGraph->unref(); - } else { + } + else { sceneGraphVRML2 = SoDB::readAllVRML(&in); if (sceneGraphVRML2 == NULL) { /*return -1;*/ } @@ -1515,7 +1553,7 @@ void vpMbTracker::loadVRMLModel(const std::string &modelFile) vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str()); throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model"); #endif -} + } void vpMbTracker::removeComment(std::ifstream &fileId) { @@ -1561,7 +1599,8 @@ std::map vpMbTracker::parseParameters(std::string &end if (pos != std::string::npos) { mapOfParams[it->first] = endLine.substr(0, pos); endLine = endLine.substr(pos + 1); - } else { + } + else { parseQuote = false; } } @@ -1662,9 +1701,10 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector> caoVersion; fileId.ignore(std::numeric_limits::max(), fileId.widen('\n')); // skip the rest of the line - } else { + } + else { std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header " - "file : use V0, V1, ..."; + "file : use V0, V1, ..."; throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter " "header file : use V0, V1, ..."); } @@ -1771,12 +1811,14 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector key(index1, index2); segmentTemporaryMap[key] = segmentInfo; - } else { + } + else { vpTRACE(" line %d has wrong coordinates.", k); } } @@ -2142,7 +2185,8 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector= VISP_CXX_STANDARD_11) std::lock_guard lock(g_mutex_cout); #endif @@ -2255,7 +2301,8 @@ void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vectorget(j); if (!strncmp(face_set->getName().getString(), "cyl", 3)) { extractCylinders(face_set, transform, idFace, name); - } else { + } + else { extractFaces(face_set, transform, idFace, name); } } @@ -2382,7 +2430,8 @@ void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatr initProjectionErrorFaceFromCorners(*(m_projectionErrorFaces.getPolygon().back())); corners.resize(0); } - } else { + } + else { coord = (SoVRMLCoordinate *)(face_set->coord.getValue()); int index = face_set->coordIndex[i]; pointTransformed[0] = coord->point[index].getValue()[0]; @@ -2445,7 +2494,8 @@ void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneous if (i < (int)corners_c1.size()) { corners_c1[(unsigned int)i] = pt; - } else { + } + else { corners_c2[(unsigned int)i - corners_c1.size()] = pt; } } @@ -2487,16 +2537,16 @@ void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneous initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName); idFace += 4; -} + } -/*! - Extract a line of the object to track from the VMRL model. This method calls - the initFaceFromCorners() method implemented in the child class. + /*! + Extract a line of the object to track from the VMRL model. This method calls + the initFaceFromCorners() method implemented in the child class. - \param line_set : Pointer to the line in the vrml format. - \param idFace : Id of the face. - \param polygonName: Name of the polygon. -*/ + \param line_set : Pointer to the line in the vrml format. + \param idFace : Id of the face. + \param polygonName: Name of the polygon. + */ void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName) { std::vector corners; @@ -2518,7 +2568,8 @@ void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, cons initProjectionErrorFaceFromCorners(*(m_projectionErrorFaces.getPolygon().back())); corners.resize(0); } - } else { + } + else { coord = (SoVRMLCoordinate *)(line_set->coord.getValue()); int index = line_set->coordIndex[i]; point[0] = coord->point[index].getValue()[0]; @@ -2593,7 +2644,8 @@ vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPo if (clipPolygon) { faces.getPolygon()[i]->getRoiClipped(m_cam, roiPts, m_cMo); - } else { + } + else { roiPts = faces.getPolygon()[i]->getRoi(m_cam, m_cMo); } @@ -2606,7 +2658,8 @@ vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPo std::vector polyPts; if (clipPolygon) { faces.getPolygon()[i]->getPolygonClipped(polyPts); - } else { + } + else { for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) { polyPts.push_back(faces.getPolygon()[i]->p[j]); } @@ -2650,22 +2703,23 @@ vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPo pairOfPolygonFaces.first = polygonsTmp; pairOfPolygonFaces.second = roisPtTmp; - } else { + } + else { pairOfPolygonFaces.first = polygonsTmp; pairOfPolygonFaces.second = roisPtTmp; } return pairOfPolygonFaces; -} + } -/*! - Use Ogre3D for visibility tests + /*! + Use Ogre3D for visibility tests - \warning This function has to be called before the initialization of the - tracker. + \warning This function has to be called before the initialization of the + tracker. - \param v : True to use it, False otherwise -*/ + \param v : True to use it, False otherwise + */ void vpMbTracker::setOgreVisibilityTest(const bool &v) { useOgre = v; @@ -2673,8 +2727,8 @@ void vpMbTracker::setOgreVisibilityTest(const bool &v) #ifndef VISP_HAVE_OGRE useOgre = false; std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test " - "will be used. setOgreVisibilityTest() set to false." - << std::endl; + "will be used. setOgreVisibilityTest() set to false." + << std::endl; #endif } } @@ -2809,7 +2863,8 @@ void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpC // computation efficiency if (isoJoIdentity) { covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D); - } else { + } + else { covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D); } } @@ -2890,7 +2945,8 @@ void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity, unsigned in v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits::epsilon()) * LTR; break; } - } else { + } + else { vpVelocityTwistMatrix cVo; cVo.buildFrom(m_cMo); vpMatrix LVJ = (L * (cVo * oJo)); @@ -2971,7 +3027,8 @@ void vpMbTracker::setEstimatedDoF(const vpColVector &v) // if(v[i] != 0){ if (std::fabs(v[i]) > std::numeric_limits::epsilon()) { oJo[i][i] = 1.0; - } else { + } + else { oJo[i][i] = 0.0; m_isoJoIdentity = false; } @@ -3326,7 +3383,7 @@ void vpMbTracker::addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) || (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) { already_here = - (std::fabs(ci->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(ci->radius, r)); + (std::fabs(ci->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(ci->radius, r)); } } @@ -3357,7 +3414,7 @@ void vpMbTracker::addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) || (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) { already_here = - (std::fabs(cy->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(cy->radius, r)); + (std::fabs(cy->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(cy->radius, r)); } } @@ -3513,8 +3570,8 @@ double vpMbTracker::computeProjectionErrorImpl(const vpImage &I, nbFeatures += lineNbFeatures; } } - } - } + } + } for (std::vector::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end(); ++it) { @@ -3557,7 +3614,7 @@ double vpMbTracker::computeProjectionErrorImpl(const vpImage &I, } return totalProjectionError; -} + } void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo) { @@ -3566,7 +3623,8 @@ void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int he if (!useOgre) { m_projectionErrorFaces.setVisible(width, height, m_projectionErrorCam, _cMo, angleAppears, angleDisappears, changed); - } else { + } + else { #ifdef VISP_HAVE_OGRE m_projectionErrorFaces.setVisibleOgre(width, height, m_projectionErrorCam, _cMo, angleAppears, angleDisappears, changed); @@ -3648,7 +3706,8 @@ void vpMbTracker::projectionErrorInitMovingEdge(const vpImage &I, l->updateTracked(); if (l->meline.empty() && l->isTracked()) l->initMovingEdge(I, _cMo, doNotTrack, m_mask); - } else { + } + else { l->setVisible(false); for (size_t a = 0; a < l->meline.size(); a++) { if (l->meline[a] != NULL) @@ -3683,7 +3742,8 @@ void vpMbTracker::projectionErrorInitMovingEdge(const vpImage &I, if (cy->isTracked()) cy->initMovingEdge(I, _cMo, doNotTrack, m_mask); } - } else { + } + else { cy->setVisible(false); if (cy->meline1 != NULL) delete cy->meline1; @@ -3716,7 +3776,8 @@ void vpMbTracker::projectionErrorInitMovingEdge(const vpImage &I, if (ci->isTracked()) ci->initMovingEdge(I, _cMo, doNotTrack, m_mask); } - } else { + } + else { ci->setVisible(false); if (ci->meEllipse != NULL) delete ci->meEllipse; @@ -3738,7 +3799,8 @@ void vpMbTracker::loadConfigFile(const std::string &configFile, bool verbose) std::cout << " *********** Parsing XML for ME projection error ************ " << std::endl; } xmlp.parse(configFile); - } catch (...) { + } + catch (...) { throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str()); } @@ -3791,4 +3853,18 @@ void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size) m_SobelY.resize(size * 2 + 1, size * 2 + 1, false, false); vpImageFilter::getSobelKernelY(m_SobelY.data, size); + + // // TODO: + // m_SobelX[0][0] = 3; m_SobelX[0][1] = 10; m_SobelX[0][2] = 3; + // m_SobelX[1][0] = 0; m_SobelX[1][1] = 0; m_SobelX[1][2] = 0; + // m_SobelX[2][0] = -3; m_SobelX[2][1] = -10; m_SobelX[2][2] = -3; + + // m_SobelY[0][0] = 3; m_SobelY[0][1] = 0; m_SobelY[0][2] = -3; + // m_SobelY[1][0] = 10; m_SobelY[1][1] = 0; m_SobelY[1][2] = -10; + // m_SobelY[2][0] = 3; m_SobelY[2][1] = 0; m_SobelY[2][2] = -3; + + // TODO: + std::cout << "---------------------" << std::endl; + std::cout << "m_SobelX:\n" << m_SobelX << std::endl; + std::cout << "m_SobelY:\n" << m_SobelY << std::endl; } diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp index 0f7254be5a..9b085c92ab 100644 --- a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp +++ b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp @@ -9,9 +9,56 @@ //! [Include] #include #include +// TODO: +#include int main(int argc, char **argv) { + { + vpRxyzVector workbench_rpy_vispa1(-1.028722, -0.249099, -2.752058); + vpHomogeneousMatrix workbench_T_vispa1(vpTranslationVector(-1.322705, 0.270946, 0.595833), vpRotationMatrix(workbench_rpy_vispa1)); + vpHomogeneousMatrix vispa1_T_workbench = workbench_T_vispa1.inverse(); + vpRxyzVector vispa1_rpy_workbench(vispa1_T_workbench.getRotationMatrix()); + + std::cout << "vispa1_t_workbench=\n" << vispa1_T_workbench.getTranslationVector() << std::endl; + std::cout << "vispa1_rpy_workbench=\n" << vispa1_rpy_workbench << std::endl; + } + { + vpRzyxVector workbench_rpy_vispa1(-1.028722, -0.249099, -2.752058); + vpHomogeneousMatrix workbench_T_vispa1(vpTranslationVector(-1.322705, 0.270946, 0.595833), vpRotationMatrix(workbench_rpy_vispa1)); + vpHomogeneousMatrix vispa1_T_workbench = workbench_T_vispa1.inverse(); + vpRzyxVector vispa1_rpy_workbench(vispa1_T_workbench.getRotationMatrix()); + + std::cout << "\n\nvispa1_t_workbench=\n" << vispa1_T_workbench.getTranslationVector() << std::endl; + std::cout << "vispa1_rpy_workbench=\n" << vispa1_rpy_workbench << std::endl; + } + // { + // vpRzyxVector base_link_rpy_vispa1(-1.028722, -0.249099, -2.752058); + // vpHomogeneousMatrix base_link_T_vispa1(vpTranslationVector(-1.322705, 0.270946, 0.595833), vpRotationMatrix(base_link_rpy_vispa1)); + // vpHomogeneousMatrix vispa1_T_base_link = base_link_T_vispa1.inverse(); + // vpRxyzVector vispa1_rpy_base_link(vispa1_T_base_link.getRotationMatrix()); + + // std::cout << "\n\nvispa1_t_base_link=\n" << vispa1_T_base_link.getTranslationVector() << std::endl; + // std::cout << "vispa1_rpy_base_link=\n" << vispa1_rpy_base_link << std::endl; + // } + { + // vpRxyzVector workbench_rpy_vispa1(-1.028722, -0.249099, -2.752058); + vpQuaternionVector workbench_rpy_vispa1(0.0116296, -0.4998696, -0.8357015, 0.2271524); + vpHomogeneousMatrix workbench_T_vispa1(vpTranslationVector(-1.322705, 0.270946, 0.595833), vpRotationMatrix(workbench_rpy_vispa1)); + vpHomogeneousMatrix base_T_workbench = workbench_T_vispa1.inverse(); + + std::cout << "\n\nbase_T_workbench=\n" << base_T_workbench.getTranslationVector() << std::endl; + vpQuaternionVector base_q_workbench(base_T_workbench.getRotationMatrix()); + std::cout << "base_q(x, y, z, w)_workbench=\n" + << base_q_workbench.x() << "\n" + << base_q_workbench.y() << "\n" + << base_q_workbench.z() << "\n" + << base_q_workbench.w() << std::endl; + } + + +// TODO: 2024-02-20 +#if 0 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_HIGHGUI) std::string opt_videoname = "model/teabox/teabox.mp4"; std::string opt_modelname = "model/teabox/teabox.cao"; @@ -304,6 +351,12 @@ int main(int argc, char **argv) //! [Set parameters] } + // TODO: + tracker.setProjectionErrorDisplay(true); + tracker.setProjectionErrorKernelSize(1); + vpImage I_gauss; + cv::Mat1b img, img_gauss; + //! [Set visibility parameters] //! [Set ogre visibility] tracker.setOgreVisibilityTest(false); @@ -388,7 +441,13 @@ int main(int argc, char **argv) } } { - double proj_error = tracker.computeCurrentProjectionError(I, cMo, cam); + // TODO: + // double proj_error = tracker.computeCurrentProjectionError(I, cMo, cam); + // vpImageFilter::gaussianBlur(I, I_gauss); + vpImageConvert::convert(I, img); + cv::GaussianBlur(img, img_gauss, cv::Size(5, 5), 0); + vpImageConvert::convert(img_gauss, I_gauss); + double proj_error = tracker.computeCurrentProjectionError(I_gauss, cMo, cam); std::stringstream ss; ss << "Projection error: " << std::setprecision(2) << proj_error << " deg"; vpDisplay::displayText(I, 80 * display->getDownScalingFactor(), 10 * display->getDownScalingFactor(), ss.str(), vpColor::red); @@ -439,6 +498,7 @@ int main(int argc, char **argv) (void)argc; (void)argv; std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl; +#endif #endif return EXIT_SUCCESS; }