From 25635f9d7c19a27f9341686755e239f9c81cb3df Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 19 Sep 2024 12:32:33 +0200 Subject: [PATCH] Fix most warnings from the initial commit --- modules/tracker/rbt/CMakeLists.txt | 2 +- .../include/visp3/rbt/vpRBDenseDepthTracker.h | 14 +- .../include/visp3/rbt/vpRBFeatureTracker.h | 1 - .../rbt/include/visp3/rbt/vpRBKltTracker.h | 11 +- .../visp3/rbt/vpRBSilhouetteCCDTracker.h | 15 +- .../visp3/rbt/vpRBSilhouetteControlPoint.h | 4 +- .../visp3/rbt/vpRBSilhouetteMeTracker.h | 9 +- .../rbt/include/visp3/rbt/vpRBTrackerLogger.h | 2 +- .../src/core/vpRBSilhouetteControlPoint.cpp | 18 +- ...vpRBSilhouettePointsExtractionSettings.cpp | 4 +- modules/tracker/rbt/src/core/vpRBTracker.cpp | 4 +- .../vpRBProbabilistic3DDriftDetector.cpp | 39 ++-- .../src/features/vpRBDenseDepthTracker.cpp | 16 +- .../rbt/src/features/vpRBFeatureTracker.cpp | 4 +- .../rbt/src/features/vpRBKltTracker.cpp | 61 ++--- .../src/features/vpRBSilhouetteCCDTracker.cpp | 215 +++++++++--------- .../src/features/vpRBSilhouetteMeTracker.cpp | 34 +-- .../tracker/rbt/src/mask/vpColorHistogram.cpp | 3 +- 18 files changed, 205 insertions(+), 251 deletions(-) diff --git a/modules/tracker/rbt/CMakeLists.txt b/modules/tracker/rbt/CMakeLists.txt index f2fe8306e7..0eaf25f726 100644 --- a/modules/tracker/rbt/CMakeLists.txt +++ b/modules/tracker/rbt/CMakeLists.txt @@ -53,7 +53,7 @@ if(WITH_SIMDLIB) list(APPEND opt_libs_private ${SIMDLIB_LIBRARIES}) endif() -vp_add_module(rbt visp_mbt visp_vision visp_core visp_me visp_visual_features visp_ar OPTIONAL visp_klt visp_gui PRIVATE_OPTIONAL ${opt_libs_private}) +vp_add_module(rbt visp_vision visp_core visp_me visp_visual_features visp_ar OPTIONAL visp_klt visp_gui PRIVATE_OPTIONAL ${opt_libs_private}) vp_glob_module_sources() vp_module_include_directories(${opt_incs}) diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h index 3550bab78a..927d6f21bc 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h @@ -39,24 +39,16 @@ #define VP_RB_DENSE_DEPTH_TRACKER_H #include -#include -#include -#include #include #include -#include -#include #include #include -#include -#include +#include #include -#include -#include + // #if defined(VISP_HAVE_SIMDLIB) // #include // #endif -#include #include #include @@ -116,9 +108,7 @@ class VISP_EXPORT vpRBDenseDepthTracker : public vpRBFeatureTracker inline void error(vpColVector &e, unsigned i) const { - double D = -((cameraNormal[0] * oP.get_X()) + (cameraNormal[1] * oP.get_Y()) + (cameraNormal[2] * oP.get_Z())); - double projNormal = cameraNormal[0] * currentPoint[0] + cameraNormal[1] * currentPoint[1] + cameraNormal[2] * currentPoint[2]; e[i] = D + projNormal; diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h index 938cc5be81..4bf4d13968 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h @@ -45,7 +45,6 @@ #include #include - #include #include diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h index 43ada59a41..d614fe21a8 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h @@ -57,7 +57,6 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker public: vpRBKltTracker(); - bool requiresRGB() const VP_OVERRIDE { return false; } bool requiresDepth() const VP_OVERRIDE { return false; } @@ -68,7 +67,6 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker void onTrackingIterEnd() VP_OVERRIDE { } - void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; @@ -112,9 +110,9 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker inline double rotationDifferenceToInitial(const vpHomogeneousMatrix &oMc) { const vpHomogeneousMatrix cinitTc = cTo0 * oMc; - return cinitTc.getThetaUVector().getTheta(); } + inline double normalDotProd(const vpHomogeneousMatrix &cMo) { vpColVector cameraNormal = cMo.getRotationMatrix() * normal; @@ -136,12 +134,6 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker e[i * 2 + 1] = oX.get_y() - currentPos.get_v(); } - inline double weight(const vpHomogeneousMatrix &cMo) - { - //return static_cast(validAndInlierCount) / static_cast(validCount); - return 1.0; - } - inline double distance(const vpTrackedKltPoint &other) const { const double d = sqrt(std::pow(oX.get_oX() - other.oX.get_oX(), 2) + @@ -150,7 +142,6 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker return d; } - inline void interaction(vpMatrix &L, unsigned i) const { double x = oX.get_x(), y = oX.get_y(); diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h index 995a47c586..03afb312b9 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h @@ -106,7 +106,6 @@ class VISP_EXPORT vpCCDParameters * maximum decrease of the covariance within one iteration step. Between 0 and 1 * If c2 is too high, the covariance declines slowly. Hence, a small number of iterations is * necessary. If c2 is too small, the CCD algorithm may converge to a wrong solution. - * * it is recommended to leave this value fixed */ double covarianceIterDecreaseFactor; @@ -115,14 +114,12 @@ class VISP_EXPORT vpCCDParameters * Length of the line along the normal (and the opposite direction). To subsample the line, set delta_h > 1. * Number of pixels used is computed as 2 * floor(h/delta_h). If you expect large motions, set a large value. * If you want to reduce computation time, decrease this value or increase delta_h - * Recommended value: above 4 - * + * Recommended value: 4 or above (this is dependent on image resolution) */ int h; /** * \brief Sample step when computing statistics and errors. * Increase this value to decrease computation time, at the risk of obtaining inacurrate statistics. - * */ int delta_h; /** @@ -145,7 +142,7 @@ inline void from_json(const nlohmann::json &j, vpCCDParameters &ccdParameters) ccdParameters.phi_dim = j.value("phi_dim", ccdParameters.phi_dim); if (j.contains("gamma")) { nlohmann::json gammaj = j["gamma"]; - if (!j.is_array() || !j.size() != 4) { + if (!j.is_array() || j.size() != 4) { throw vpException(vpException::ioError, "CCD parameters: tried to read gamma values from something that is not a 4-sized float array"); } ccdParameters.gamma_1 = gammaj[0]; @@ -179,10 +176,6 @@ class VISP_EXPORT vpCCDStatistics }; -/** - * \brief A base class for all features that can be used and tracker in the vpRenderBasedTracker - * - */ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker { public: @@ -224,11 +217,11 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker double getVVSTrackerWeight() const VP_OVERRIDE { return m_userVvsWeight / (10 * error_ccd.size()); } void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; - void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE { } + void trackFeatures(const vpRBFeatureTrackerInput & /*frame*/, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix & /*cMo*/) VP_OVERRIDE { } void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; - void updateCovariance(const double lambda) VP_OVERRIDE { } + void updateCovariance(const double /*lambda*/) VP_OVERRIDE { } void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE; diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h index 495a65b344..9921fc4540 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h @@ -166,8 +166,8 @@ class VISP_EXPORT vpRBSilhouetteControlPoint void trackMultipleHypotheses(const vpImage &I); void initInteractionMatrixError(); - void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage &I); - void computeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo, const vpImage &I); + void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo); + void computeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo); private: void sample(const vpImage &) { } diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h index 06305ea189..1e6034d14d 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h @@ -39,14 +39,13 @@ #include #include -#include #include class VISP_EXPORT vpRBSilhouetteMeTracker : public vpRBFeatureTracker { public: - vpRBSilhouetteMeTracker() : vpRBFeatureTracker(), m_me(), m_numCandidates(1), m_globalVVSConvergenceThreshold(1.0), m_singlePointConvergedThresholdPixels(3), m_minMaskConfidence(0.f), m_useMask(false) { } + vpRBSilhouetteMeTracker() : vpRBFeatureTracker(), m_me(), m_numCandidates(1), m_globalVVSConvergenceThreshold(1.0), m_singlePointConvergedThresholdPixels(3), m_useMask(false), m_minMaskConfidence(0.f) { } bool requiresRGB() const VP_OVERRIDE { return false; } @@ -68,8 +67,6 @@ class VISP_EXPORT vpRBSilhouetteMeTracker : public vpRBFeatureTracker */ void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; - void clusterIntoLines(); - void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; @@ -98,9 +95,9 @@ class VISP_EXPORT vpRBSilhouetteMeTracker : public vpRBFeatureTracker std::vector m_controlPoints; vpMe m_me; //! Moving edge settings unsigned int m_numCandidates; //! Number of best candidates kept when finding correspondence points - double m_singlePointConvergedThresholdPixels; //! Whether a single Control point is considered as converged - double m_globalVVSConvergenceThreshold; //! Percentage of control points that should have converged to consider VVS as successful vpRobust m_robust; //! M-Estimator to filter outliers + double m_globalVVSConvergenceThreshold; //! Percentage of control points that should have converged to consider VVS as successful + double m_singlePointConvergedThresholdPixels; //! Whether a single Control point is considered as converged bool m_useMask; float m_minMaskConfidence; diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h index a72aa6dab4..c81309f8f9 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h @@ -130,7 +130,7 @@ std::ostream &operator<<(std::ostream &out, const vpRBTrackerLogger &timer) out << "Silhouette extraction: " << timer.m_silhouetteExtractionTime << "ms" << std::endl; out << "Trackers: " << std::endl; - for (const std::pair> vvsIterData : timer.m_trackerVVSIterTimes) { + for (const std::pair> &vvsIterData : timer.m_trackerVVSIterTimes) { double featTrackTime = timer.m_trackerFeatureTrackingTime.find(vvsIterData.first)->second; double featExtractionTime = timer.m_trackerFeatureExtractionTime.find(vvsIterData.first)->second; double initVVSTime = timer.m_trackerInitVVSTime.find(vvsIterData.first)->second; diff --git a/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp index 17643452e1..0709388754 100644 --- a/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp +++ b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp @@ -165,8 +165,8 @@ void vpRBSilhouetteControlPoint::track(const vpImage &I) s.track(I, me, false); } } - catch (vpTrackingException) { - vpERROR_TRACE("catch exception "); + catch (vpTrackingException &) { + vpERROR_TRACE("caught a tracking exception, ignoring me point..."); s.setState(vpMeSite::THRESHOLD); } } @@ -178,12 +178,12 @@ void vpRBSilhouetteControlPoint::trackMultipleHypotheses(const vpImage &/*I*/) +vpRBSilhouetteControlPoint::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo) { line.changeFrame(cMo); @@ -496,7 +492,7 @@ vpRBSilhouetteControlPoint::computeInteractionMatrixError(const vpHomogeneousMat } void -vpRBSilhouetteControlPoint::computeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo, const vpImage &/*I*/) +vpRBSilhouetteControlPoint::computeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo) { line.changeFrame(cMo); diff --git a/modules/tracker/rbt/src/core/vpRBSilhouettePointsExtractionSettings.cpp b/modules/tracker/rbt/src/core/vpRBSilhouettePointsExtractionSettings.cpp index 9a653269ae..1e3b6f028e 100644 --- a/modules/tracker/rbt/src/core/vpRBSilhouettePointsExtractionSettings.cpp +++ b/modules/tracker/rbt/src/core/vpRBSilhouettePointsExtractionSettings.cpp @@ -84,7 +84,7 @@ std::vector> vpSilhouettePointsExtractionS } if (m_preferPreviousPoints) { for (const vpRBSilhouettePoint &p: previousPoints) { - double x, y; + double x = 0.0, y = 0.0; vpPixelMeterConversion::convertPoint(cam, p.j, p.i, x, y); vpColVector cpX({ x * p.Z, y * p.Z, p.Z, 1.0 }); vpColVector cX = cTcp * cpX; @@ -100,7 +100,7 @@ std::vector> vpSilhouettePointsExtractionS } } } - if (m_maxNumPoints > 0 && finalCandidates.size() >= m_maxNumPoints) { + if (m_maxNumPoints > 0 && finalCandidates.size() >= static_cast(m_maxNumPoints)) { return finalCandidates; } diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index 9860323d42..b3e11b93b3 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -52,8 +52,7 @@ #define VP_DEBUG_RB_TRACKER 1 -vpRBTracker::vpRBTracker() : m_lambda(1.0), m_vvsIterations(10), m_muInit(0.0), m_muIterFactor(0.5), m_imageHeight(480), m_imageWidth(640), -m_firstIteration(true), m_renderer(m_rendererSettings), m_trackers(0) +vpRBTracker::vpRBTracker() : m_firstIteration(true), m_trackers(0), m_lambda(1.0), m_vvsIterations(10), m_muInit(0.0), m_muIterFactor(0.5), m_renderer(m_rendererSettings), m_imageHeight(480), m_imageWidth(640) { m_rendererSettings.setClippingDistance(0.01, 1.0); const std::shared_ptr geometryRenderer = std::make_shared( @@ -283,7 +282,6 @@ void vpRBTracker::track(vpRBFeatureTrackerInput &input) error += (weight * tracker->getWeightedError()).sumSquare(); //std::cout << "Error = " << (weight * tracker->getWeightedError()).sumSquare() << std::endl; } - } if (numFeatures >= 6) { diff --git a/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp index 1be086747e..c2f4fd4aec 100644 --- a/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp +++ b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp @@ -33,22 +33,20 @@ #include #include - -#include #include #include +#include + #if defined(VISP_HAVE_NLOHMANN_JSON) #include #endif void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) { - const vpTranslationVector t = cprevTo.inverse().getTranslationVector(); if (m_points.size() > 0) { - // Step 0: project all points #ifdef VISP_HAVE_OPENMP #pragma omp parallel for @@ -62,10 +60,10 @@ void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &pre for (vpStored3DSurfaceColorPoint &p : m_points) { p.visible = true; if ( - p.projPrevPx[0] < 2 || p.projPrevPx[0] >= frame.IRGB.getWidth() - 2 - || p.projPrevPx[1] < 2 || p.projPrevPx[1] >= frame.IRGB.getHeight() - 2 - || p.projCurrPx[0] < 2 || p.projCurrPx[0] >= frame.IRGB.getWidth() - 2 - || p.projCurrPx[1] < 2 || p.projCurrPx[1] >= frame.IRGB.getHeight() - 2) { + p.projPrevPx[0] < 2 || static_cast(p.projPrevPx[0]) >= frame.IRGB.getWidth() - 2 + || p.projPrevPx[1] < 2 || static_cast(p.projPrevPx[1]) >= frame.IRGB.getHeight() - 2 + || p.projCurrPx[0] < 2 || static_cast(p.projCurrPx[0]) >= frame.IRGB.getWidth() - 2 + || p.projCurrPx[1] < 2 || static_cast(p.projCurrPx[1]) >= frame.IRGB.getHeight() - 2) { p.visible = false; // Point is outside of either current or previous image, ignore it continue; } @@ -109,18 +107,17 @@ void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &pre } } - if (visiblePoints.size() > 0) { - // Compute sample weight - double maxTrace = 0.0; - - for (vpStored3DSurfaceColorPoint *p : visiblePoints) { - double trace = p->stats.trace(); - if (trace > maxTrace) { - maxTrace = trace; - } - } - maxTrace = std::max(maxTrace, 80.0); + // // Compute sample weight + // double maxTrace = 0.0; + + // for (vpStored3DSurfaceColorPoint *p : visiblePoints) { + // double trace = p->stats.trace(); + // if (trace > maxTrace) { + // maxTrace = trace; + // } + // } + // maxTrace = std::max(maxTrace, 80.0); double weightSum = 0.0; m_score = 0.0; for (vpStored3DSurfaceColorPoint *p : visiblePoints) { @@ -183,8 +180,8 @@ void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &pre for (unsigned int i = frame.renders.boundingBox.getTop(); i < frame.renders.boundingBox.getBottom(); i += 2) { for (unsigned int j = frame.renders.boundingBox.getLeft(); j < frame.renders.boundingBox.getRight(); j += 2) { - double u = j, v = i; - double x, y; + double u = static_cast(j), v = static_cast(i); + double x = 0.0, y = 0.0; double Z = frame.renders.depth[i][j]; if (Z > 0.f) { vpPixelMeterConversion::convertPoint(frame.cam, u, v, x, y); diff --git a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp index 9b004dadeb..27651d32cf 100644 --- a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp @@ -54,7 +54,7 @@ void fastProjection(const vpHomogeneousMatrix &oTc, double X, double Y, double Z p.set_oW(1.0); } -void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &cMo) { double t1 = vpTime::measureTimeMs(); const vpImage &depthMap = frame.depth; @@ -62,7 +62,7 @@ void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame vpRect bb = frame.renders.boundingBox; vpHomogeneousMatrix oMc = cMo.inverse(); vpRotationMatrix cRo = cMo.getRotationMatrix(); - bool useMask = m_useMask && frame.mask.getSize() > 0; + bool useMask = m_useMask && frame.hasMask(); m_depthPoints.clear(); m_depthPoints.reserve(static_cast(bb.getArea() / (m_step * m_step * 2))); vpDepthPoint point; @@ -75,7 +75,7 @@ void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame if (useMask && frame.mask[i][j] < m_minMaskConfidence) { continue; } - double x, y; + double x = 0.0, y = 0.0; vpPixelMeterConversion::convertPoint(frame.cam, j, i, x, y); //vpColVector objectNormal({ frame.renders.normals[i][j].R, frame.renders.normals[i][j].G, frame.renders.normals[i][j].B }); point.objectNormal[0] = frame.renders.normals[i][j].R; @@ -114,7 +114,7 @@ void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame } -void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) +void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, const vpHomogeneousMatrix &cMo, unsigned int /*iteration*/) { if (m_numFeatures == 0) { m_LTL = 0; @@ -123,7 +123,6 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, m_weights = 1.0; m_weighted_error = 0.0; } - double t1 = vpTime::measureTimeMs(); vpRotationMatrix cRo = cMo.getRotationMatrix(); #ifdef VISP_HAVE_OPENMP #pragma omp parallel for @@ -135,9 +134,8 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, depthPoint.interaction(m_L, i); } -//m_weights = 0.0; -//m_robust.setMinMedianAbsoluteDeviation(1e-3); - t1 = vpTime::measureTimeMs(); + //m_weights = 0.0; + //m_robust.setMinMedianAbsoluteDeviation(1e-3); m_robust.MEstimator(vpRobust::TUKEY, m_error, m_weights); for (unsigned int i = 0; i < m_depthPoints.size(); ++i) { m_weighted_error[i] = m_error[i] * m_weights[i]; @@ -151,7 +149,7 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, m_vvsConverged = false; } -void vpRBDenseDepthTracker::display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const +void vpRBDenseDepthTracker::display(const vpCameraParameters &/*cam*/, const vpImage &/*I*/, const vpImage &/*IRGB*/, const vpImage &/*depth*/, const vpRBFeatureDisplayType /*type*/) const { // for (unsigned int i = 0; i < m_depthPoints.size(); ++i) { // const vpDepthPoint &p = m_depthPoints[i]; diff --git a/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp b/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp index 6b01904cc9..0791df50c5 100644 --- a/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp @@ -43,8 +43,6 @@ vpRBFeatureTracker::vpRBFeatureTracker() m_vvsConverged = false; } - - void vpRBFeatureTracker::updateCovariance(const double lambda) { vpMatrix D; @@ -72,7 +70,7 @@ void vpRBFeatureTracker::computeJTR(const vpMatrix &interaction, const vpColVect ssum += interaction[j][i] * error[j]; } JTR[i] = ssum; -} + } #endif } diff --git a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp index 3eea86261e..fe21735fe2 100644 --- a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp @@ -56,11 +56,11 @@ const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc) if (Z <= 0.f || (frame.hasDepth() && frame.depth[uv][uu] > 0.f && fabs(frame.depth[uv][uu] - Z) > 5e-3)) { return; } - double x, y; vpRBKltTracker::vpTrackedKltPoint p; p.cTo0 = cMo; vpRGBf normalRGB = frame.renders.normals[uv][uu]; p.normal = vpColVector({ normalRGB.R, normalRGB.G, normalRGB.B }); + double x = 0.0, y = 0.0; vpPixelMeterConversion::convertPoint(frame.cam, static_cast(u), static_cast(v), x, y); vpColVector oC({ x * Z, y * Z, Z, 1.0 }); vpColVector oX = oMc * oC; @@ -79,18 +79,19 @@ vpRBKltTracker::vpRBKltTracker() : } -void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &cMo) { m_Iprev = m_I; vpImageConvert::convert(frame.I, m_I); const vpHomogeneousMatrix oMc = cMo.inverse(); if (m_maxErrorOutliersPixels > 0.0) { - double distanceThresholdPxSquare = vpMath::sqr(m_maxErrorOutliersPixels); + const double distanceThresholdPxSquare = vpMath::sqr(m_maxErrorOutliersPixels); + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); // Detect outliers - for (unsigned int i = 0; i < m_klt.getNbFeatures(); ++i) { - long id; - float u, v; - double x, y; + for (unsigned int i = 0; i < nbFeatures; ++i) { + long id = 0; + float u = 0.f, v = 0.f; + m_klt.getFeature(i, id, u, v); if (m_points.find(id) != m_points.end()) { unsigned int uu = static_cast(round(u)), uv = static_cast(round(v)); @@ -101,17 +102,18 @@ void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const float Z = frame.renders.depth[uv][uu]; if (Z > 0.f) { vpTrackedKltPoint &p = m_points[id]; + double x = 0.0, y = 0.0; vpPixelMeterConversion::convertPoint(frame.cam, static_cast(u), static_cast(v), x, y); vpColVector oXn = oMc * vpColVector({ x * Z, y * Z, Z, 1.0 }); oXn /= oXn[3]; p.update(cMo); double x1 = p.oX.get_x(), y1 = p.oX.get_y(); - double u1, v1; + double u1 = 0.0, v1 = 0.0; vpMeterPixelConversion::convertPoint(frame.cam, x1, y1, u1, v1); double distancePx = vpMath::sqr(u1 - u) + vpMath::sqr(v1 - v); vpColVector oX = p.oX.get_oP(); - if (distancePx > m_maxErrorOutliersPixels) { + if (distancePx > distanceThresholdPxSquare) { m_points.erase(id); m_klt.suppressFeature(i); } @@ -122,9 +124,8 @@ void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const cv::Mat mask = cv::Mat::zeros(m_I.rows, m_I.cols, CV_8U); vpRect bb = frame.renders.boundingBox; - double timeBeforeMask = vpTime::measureTimeMs(); - for (unsigned int i = bb.getTop(); i < bb.getBottom(); ++i) { - for (unsigned int j = bb.getLeft(); j < bb.getRight(); ++j) { + for (unsigned int i = static_cast(bb.getTop()); i < static_cast(bb.getBottom()); ++i) { + for (unsigned int j = static_cast(bb.getLeft()); j < static_cast(bb.getRight()); ++j) { mask.at(i, j) = (frame.renders.depth[i][j] > 0.f) * 255; } } @@ -133,8 +134,9 @@ void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const // Consider that there are not enough points: reinit KLT tracking if (m_points.size() < m_numPointsReinit) { m_klt.initTracking(m_Iprev, mask); + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); m_points.clear(); - for (unsigned int i = 0; i < m_klt.getNbFeatures(); ++i) { + for (unsigned int i = 0; i < nbFeatures; ++i) { long id; float u, v; m_klt.getFeature(i, id, u, v); @@ -151,14 +153,15 @@ void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const kltTemp.setBlockSize(m_klt.getBlockSize()); kltTemp.setPyramidLevels(m_klt.getPyramidLevels()); kltTemp.initTracking(m_Iprev, mask); - - for (unsigned int i = 0; i < kltTemp.getNbFeatures(); ++i) { + const unsigned int nbFeaturesTemp = static_cast(kltTemp.getNbFeatures()); + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); + for (unsigned int i = 0; i < nbFeaturesTemp; ++i) { double threshold = vpMath::sqr(m_newPointsDistanceThreshold); // distance threshold, in squared pixels double tooClose = false; float u, v; long id; kltTemp.getFeature(i, id, u, v); - for (unsigned int j = 0; j < m_klt.getNbFeatures(); ++j) { + for (unsigned int j = 0; j < nbFeatures; ++j) { float uj, vj; long idj; m_klt.getFeature(j, idj, uj, vj); @@ -182,7 +185,8 @@ void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const else { m_klt.initTracking(m_I, mask); m_points.clear(); - for (unsigned int i = 0; i < m_klt.getNbFeatures(); ++i) { + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); + for (unsigned int i = 0; i < nbFeatures; ++i) { long id; float u, v; m_klt.getFeature(i, id, u, v); @@ -191,18 +195,19 @@ void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const } } -void vpRBKltTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBKltTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &cMo) { - if (m_klt.getNbFeatures() > 0) { + const unsigned int nbKltFeatures = static_cast(m_klt.getNbFeatures()); + if (nbKltFeatures > 0) { m_klt.track(m_I); } std::map newPoints; const vpHomogeneousMatrix oMc = cMo.inverse(); - for (unsigned int i = 0; i < m_klt.getNbFeatures(); ++i) { - long id; - float u, v; - double x, y; + for (unsigned int i = 0; i < nbKltFeatures; ++i) { + long id = 0; + float u = 0.f, v = 0.f; + double x = 0.0, y = 0.0; m_klt.getFeature(i, id, u, v); unsigned int uu = static_cast(round(u)), uv = static_cast(round(v)); if (isTooCloseToBorder(uv, uu, frame.renders.depth.getRows(), frame.renders.depth.getCols(), m_border)) { @@ -234,7 +239,7 @@ void vpRBKltTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const v m_numFeatures = m_points.size() * 2; } -void vpRBKltTracker::initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBKltTracker::initVVS(const vpRBFeatureTrackerInput &/*frame*/, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix & /*cMo*/) { if (m_numFeatures < m_numPointsReinit * 2) { m_numFeatures = 0; @@ -249,7 +254,7 @@ void vpRBKltTracker::initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFea m_error = 0; } -void vpRBKltTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) +void vpRBKltTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, const vpHomogeneousMatrix &cMo, unsigned int /*iteration*/) { if (m_numFeatures < m_numPointsReinit * 2) { m_LTL = 0; @@ -281,10 +286,10 @@ void vpRBKltTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, const } -void vpRBKltTracker::display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const +void vpRBKltTracker::display(const vpCameraParameters &cam, const vpImage &I, const vpImage &/*IRGB*/, const vpImage &/*depth*/, const vpRBFeatureDisplayType /*type*/) const { - for (const std::pair &p : m_points) { - double u, v; + for (const std::pair &p : m_points) { + double u = 0.0, v = 0.0; vpMeterPixelConversion::convertPoint(cam, p.second.currentPos.get_j(), p.second.currentPos.get_i(), u, v); vpDisplay::displayPoint(I, v, u, vpColor::red, 2); } diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp index e5e9a8888a..fcd52665d0 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp @@ -31,6 +31,7 @@ * *****************************************************************************/ +#include #include #ifdef VISP_HAVE_OPENMP @@ -85,12 +86,8 @@ template class FastMat33 vpRBSilhouetteCCDTracker::vpRBSilhouetteCCDTracker() : vpRBFeatureTracker(), m_vvsConvergenceThreshold(0.0), m_temporalSmoothingFac(0.1) { } -void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &cMo) { - const unsigned rows = frame.I.getRows(), cols = frame.I.getCols(); - float sceneSize = frame.renders.zFar - frame.renders.zNear; - - m_controlPoints.clear(); //m_controlPoints.reserve(frame.silhouettePoints.size()); const vpHomogeneousMatrix oMc = cMo.inverse(); @@ -111,7 +108,7 @@ void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &fr -void vpRBSilhouetteCCDTracker::initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBSilhouetteCCDTracker::initVVS(const vpRBFeatureTrackerInput &/*frame*/, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix & /*cMo*/) { // Reinit all variables Sigma_Phi = vpMatrix(m_ccdParameters.phi_dim, m_ccdParameters.phi_dim, 0.0); @@ -158,7 +155,6 @@ void vpRBSilhouetteCCDTracker::computeVVSIter(const vpRBFeatureTrackerInput &fra tol += abs(oldPoints[i * 2 + 1] - m_controlPoints[i].icpoint.get_v()); } tol /= m_controlPoints.size(); - double t1 = vpTime::measureTimeMs(); computeLocalStatistics(frame.IRGB, m_stats); computeErrorAndInteractionMatrix(); // Update interaction matrix, and gauss newton left and right side terms @@ -170,7 +166,7 @@ void vpRBSilhouetteCCDTracker::computeVVSIter(const vpRBFeatureTrackerInput &fra -void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const +void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &/*cam*/, const vpImage &/*I*/, const vpImage &IRGB, const vpImage &/*depth*/, const vpRBFeatureDisplayType type) const { unsigned normal_points_number = floor(m_ccdParameters.h / m_ccdParameters.delta_h); unsigned nerror_per_point = 2 * normal_points_number * 3; @@ -190,7 +186,7 @@ void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &cam, const vpIm double maxPointError = 0.0; for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { double sum = 0.0; - for (unsigned j = 0; j < nerror_per_point; ++j) { + for (unsigned int j = 0; j < nerror_per_point; ++j) { sum += error_ccd[i * nerror_per_point + j]; } if (sum > maxPointError) { @@ -220,7 +216,7 @@ void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &cam, const vpIm vpColVector weightPerPoint(m_controlPoints.size()); for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { double sum = 0.0; - for (unsigned j = 0; j < nerror_per_point; ++j) { + for (unsigned int j = 0; j < nerror_per_point; ++j) { sum += m_weights[i * nerror_per_point + j]; } @@ -297,7 +293,6 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, continue; } double *nv_ptr = stats.nv[kk]; - double *weight_ptr = stats.weight[kk]; nv_ptr[0] = p.nxs; nv_ptr[1] = p.nys; #if VISP_DEBUG_CCD_TRACKER @@ -384,120 +379,121 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, } } - VISP_OPENMP(parallel for) - for (unsigned int i = 0; i < resolution; ++i) { - if (!m_controlPoints[i].isValid()) { - continue; - } - - int k = 0; - // w1 = \sum wp_1, w2 = \sum wp_2 - double w1 = 0.0, w2 = 0.0; +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int i = 0; i < resolution; ++i) { + if (!m_controlPoints[i].isValid()) { + continue; + } - // store mean value near the curve - std::array m1 { 0.0, 0.0, 0.0 }, m2 { 0.0, 0.0, 0.0 }; + int k = 0; + // w1 = \sum wp_1, w2 = \sum wp_2 + double w1 = 0.0, w2 = 0.0; - // store the second mean value near the curve - std::array m1_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; - std::array m2_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + // store mean value near the curve + std::array m1 { 0.0, 0.0, 0.0 }, m2 { 0.0, 0.0, 0.0 }; - // compute local statistics + // store the second mean value near the curve + std::array m1_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + std::array m2_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; - // start search the points in the +n direction as well as -n direction - double wp1 = 0.0, wp2 = 0.0; + // compute local statistics - double *vic_ptr = stats.vic[i]; - double *mean_vic_ptr = stats.mean_vic[i]; - double *cov_vic_ptr = stats.cov_vic[i]; - double *pix_ptr = stats.imgPoints[i]; - double *weight_ptr = stats.weight[i]; + // start search the points in the +n direction as well as -n direction + double wp1 = 0.0, wp2 = 0.0; - for (int j = m_ccdParameters.delta_h; j <= m_ccdParameters.h; j += m_ccdParameters.delta_h, k++) { - wp1 = 0.0, wp2 = 0.0; - int negative_normal = k + (int)floor(m_ccdParameters.h / m_ccdParameters.delta_h); - const double *vic_k = vic_ptr + 10 * k; + double *vic_ptr = stats.vic[i]; + double *mean_vic_ptr = stats.mean_vic[i]; + double *cov_vic_ptr = stats.cov_vic[i]; + double *pix_ptr = stats.imgPoints[i]; - // wp1 = w(a_{k,l})*w(d_{k,l})*w(d) - wp1 = (vic_k[5] * vic_k[7] / normalized_param[i][0]); + for (int j = m_ccdParameters.delta_h; j <= m_ccdParameters.h; j += m_ccdParameters.delta_h, k++) { + wp1 = 0.0, wp2 = 0.0; + int negative_normal = k + (int)floor(m_ccdParameters.h / m_ccdParameters.delta_h); + const double *vic_k = vic_ptr + 10 * k; - // wp2 = w(a_{k,l})*w(d_{k,l})*w(d) - wp2 = (vic_k[6] * vic_k[7] / normalized_param[i][1]); - //w1 = \sum{wp1} - w1 += wp1; + // wp1 = w(a_{k,l})*w(d_{k,l})*w(d) + wp1 = (vic_k[5] * vic_k[7] / normalized_param[i][0]); - //w2 = \sum{wp2} - w2 += wp2; + // wp2 = w(a_{k,l})*w(d_{k,l})*w(d) + wp2 = (vic_k[6] * vic_k[7] / normalized_param[i][1]); + //w1 = \sum{wp1} + w1 += wp1; - // compute the mean value in the vicinity of a point - // m_{ks} = I{k}^{s} = \sum_{l} w_{kls}{I_{kl}} : s = 1 or 2 - const vpRGBa pixelRGBa = I(vic_k[0], vic_k[1]); - double *pixel = pix_ptr + k * 3; - pixel[0] = pixelRGBa.R; - pixel[1] = pixelRGBa.G; - pixel[2] = pixelRGBa.B; + //w2 = \sum{wp2} + w2 += wp2; - m1[0] += wp1 * pixel[0]; - m1[1] += wp1 * pixel[1]; - m1[2] += wp1 * pixel[2]; + // compute the mean value in the vicinity of a point + // m_{ks} = I{k}^{s} = \sum_{l} w_{kls}{I_{kl}} : s = 1 or 2 + const vpRGBa pixelRGBa = I(vic_k[0], vic_k[1]); + double *pixel = pix_ptr + k * 3; + pixel[0] = pixelRGBa.R; + pixel[1] = pixelRGBa.G; + pixel[2] = pixelRGBa.B; - m2[0] += wp2 * pixel[0]; - m2[1] += wp2 * pixel[1]; - m2[2] += wp2 * pixel[2]; + m1[0] += wp1 * pixel[0]; + m1[1] += wp1 * pixel[1]; + m1[2] += wp1 * pixel[2]; + m2[0] += wp2 * pixel[0]; + m2[1] += wp2 * pixel[1]; + m2[2] += wp2 * pixel[2]; - // compute second order local statistics - // m_{k,s} = \sum_{l} w_{kls} I_{kl}*I_{kl}^T - for (unsigned int m = 0; m < 3; ++m) { - for (unsigned int n = 0; n < 3; ++n) { - m1_o2[m * 3 + n] += wp1 * pixel[m] * pixel[n]; - m2_o2[m * 3 + n] += wp2 * pixel[m] * pixel[n]; - } - } - const double *vic_neg = vic_ptr + 10 * negative_normal; - const vpRGBa pixelNegRGBa = I(vic_neg[0], vic_neg[1]); - double *pixelNeg = pix_ptr + negative_normal * 3; - - pixelNeg[0] = pixelNegRGBa.R; - pixelNeg[1] = pixelNegRGBa.G; - pixelNeg[2] = pixelNegRGBa.B; - wp1 = (vic_neg[5] * vic_neg[7] / normalized_param[i][0]); - wp2 = (vic_neg[6] * vic_neg[7] / normalized_param[i][1]); - w1 += wp1; - w2 += wp2; - - m1[0] += wp1 * pixelNeg[0]; - m1[1] += wp1 * pixelNeg[1]; - m1[2] += wp1 * pixelNeg[2]; - - m2[0] += wp2 * pixelNeg[0]; - m2[1] += wp2 * pixelNeg[1]; - m2[2] += wp2 * pixelNeg[2]; - for (int m = 0; m < 3; ++m) { - for (int n = 0; n < 3; ++n) { - m1_o2[m * 3 + n] += wp1 * pixelNeg[m] * pixelNeg[n]; - m2_o2[m * 3 + n] += wp2 * pixelNeg[m] * pixelNeg[n]; - } + // compute second order local statistics + // m_{k,s} = \sum_{l} w_{kls} I_{kl}*I_{kl}^T + for (unsigned int m = 0; m < 3; ++m) { + for (unsigned int n = 0; n < 3; ++n) { + m1_o2[m * 3 + n] += wp1 * pixel[m] * pixel[n]; + m2_o2[m * 3 + n] += wp2 * pixel[m] * pixel[n]; } } - mean_vic_ptr[0] = m1[0] / w1; - mean_vic_ptr[1] = m1[1] / w1; - mean_vic_ptr[2] = m1[2] / w1; - - mean_vic_ptr[3] = m2[0] / w2; - mean_vic_ptr[4] = m2[1] / w2; - mean_vic_ptr[5] = m2[2] / w2; + const double *vic_neg = vic_ptr + 10 * negative_normal; + const vpRGBa pixelNegRGBa = I(vic_neg[0], vic_neg[1]); + double *pixelNeg = pix_ptr + negative_normal * 3; + + pixelNeg[0] = pixelNegRGBa.R; + pixelNeg[1] = pixelNegRGBa.G; + pixelNeg[2] = pixelNegRGBa.B; + wp1 = (vic_neg[5] * vic_neg[7] / normalized_param[i][0]); + wp2 = (vic_neg[6] * vic_neg[7] / normalized_param[i][1]); + w1 += wp1; + w2 += wp2; + + m1[0] += wp1 * pixelNeg[0]; + m1[1] += wp1 * pixelNeg[1]; + m1[2] += wp1 * pixelNeg[2]; + + m2[0] += wp2 * pixelNeg[0]; + m2[1] += wp2 * pixelNeg[1]; + m2[2] += wp2 * pixelNeg[2]; for (unsigned int m = 0; m < 3; ++m) { for (unsigned int n = 0; n < 3; ++n) { - cov_vic_ptr[m * 3 + n] = m1_o2[m * 3 + n] / w1 - m1[m] * m1[n] / (w1 * w1); - cov_vic_ptr[9 + m * 3 + n] = m2_o2[m * 3 + n] / w2 - m2[m] * m2[n] / (w2 * w2); + m1_o2[m * 3 + n] += wp1 * pixelNeg[m] * pixelNeg[n]; + m2_o2[m * 3 + n] += wp2 * pixelNeg[m] * pixelNeg[n]; } - cov_vic_ptr[m * 3 + m] += m_ccdParameters.kappa; - cov_vic_ptr[9 + m * 3 + m] += m_ccdParameters.kappa; } - } + mean_vic_ptr[0] = m1[0] / w1; + mean_vic_ptr[1] = m1[1] / w1; + mean_vic_ptr[2] = m1[2] / w1; + + mean_vic_ptr[3] = m2[0] / w2; + mean_vic_ptr[4] = m2[1] / w2; + mean_vic_ptr[5] = m2[2] / w2; + + for (unsigned int m = 0; m < 3; ++m) { + for (unsigned int n = 0; n < 3; ++n) { + cov_vic_ptr[m * 3 + n] = m1_o2[m * 3 + n] / w1 - m1[m] * m1[n] / (w1 * w1); + cov_vic_ptr[9 + m * 3 + n] = m2_o2[m * 3 + n] / w2 - m2[m] * m2[n] / (w2 * w2); + } + cov_vic_ptr[m * 3 + m] += m_ccdParameters.kappa; + cov_vic_ptr[9 + m * 3 + m] += m_ccdParameters.kappa; + } + + } } void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() @@ -530,8 +526,8 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() const vpRBSilhouetteControlPoint &p = m_controlPoints[kk]; if (!p.isValid()) { - for (int j = 0; j < 2 * normal_points_number; ++j) { - for (int m = 0; m < 3; ++m) { + for (unsigned int j = 0; j < 2 * normal_points_number; ++j) { + for (unsigned int m = 0; m < 3; ++m) { error_ccd[i * 2 * normal_points_number * 3 + j * 3 + m] = 0.0; } } @@ -543,7 +539,6 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() const double *mean_vic_ptr = m_stats.mean_vic[i]; const double *cov_vic_ptr = m_stats.cov_vic[i]; const double *pix_ptr = m_stats.imgPoints[i]; - const double *weight_ptr = m_stats.weight[i]; const double *mean_vic_ptr_prev = m_prevStats.mean_vic[i]; const double *cov_vic_ptr_prev = m_prevStats.cov_vic[i]; @@ -557,7 +552,7 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() Lnvp[5] = (nv_ptr[0] * p.ys - nv_ptr[1] * p.xs); - for (int j = 0; j < 2 * normal_points_number; ++j) { + for (unsigned int j = 0; j < 2 * normal_points_number; ++j) { const double *vic_j = vic_ptr + 10 * j; const double *pix_j = pix_ptr + j * 3; const double errf = vic_j[4]; @@ -599,7 +594,7 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() } } } - double afterParallel = vpTime::measureTimeMs(); + nabla_E = 0.0; hessian_E = 0.0; //m_robust.setMinMedianAbsoluteDeviation(1.0); @@ -612,9 +607,8 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() } } - - - std::vector localGradients; // Store all the gradients and hessians and then sum them up after the parallel region. This ensures that computation is determinist + // Store all the gradients and hessians and then sum them up after the parallel region. This ensures that computation is determinist + std::vector localGradients; std::vector localHessians; #ifdef VISP_HAVE_OPENMP #pragma omp parallel @@ -655,7 +649,6 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() nabla_E += localGradients[i]; hessian_E += localHessians[i]; } - double afterWeight = vpTime::measureTimeMs(); m_LTL = hessian_E; m_LTR = -nabla_E; diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp index 79b37a3498..1ae845860a 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp @@ -85,7 +85,7 @@ void vpRBSilhouetteMeTracker::extractFeatures(const vpRBFeatureTrackerInput &fra } -void vpRBSilhouetteMeTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBSilhouetteMeTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) { if (m_numCandidates <= 1) { for (vpRBSilhouetteControlPoint &p: m_controlPoints) { @@ -99,7 +99,7 @@ void vpRBSilhouetteMeTracker::trackFeatures(const vpRBFeatureTrackerInput &frame } } -void vpRBSilhouetteMeTracker::initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBSilhouetteMeTracker::initVVS(const vpRBFeatureTrackerInput & /*frame*/, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix & /*cMo*/) { if (m_numFeatures == 0) { return; @@ -118,7 +118,7 @@ void vpRBSilhouetteMeTracker::initVVS(const vpRBFeatureTrackerInput &frame, cons m_error.resize(m_numFeatures, false); } -void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) +void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int /*iteration*/) { vpColVector factor(m_numFeatures, 1.0); const double threshold = m_singlePointConvergedThresholdPixels / frame.cam.get_px(); //Transformation limite pixel en limite metre. @@ -130,10 +130,10 @@ void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &fram vpRBSilhouetteControlPoint &p = m_controlPoints[k]; //p.update(cMo); if (m_numCandidates <= 1) { - p.computeInteractionMatrixError(cMo, frame.I); + p.computeInteractionMatrixError(cMo); } else { - p.computeInteractionMatrixErrorMH(cMo, frame.I); + p.computeInteractionMatrixErrorMH(cMo); } @@ -164,12 +164,12 @@ void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &fram const double percentageConverged = (double)count / (double)countValidSites; if (percentageConverged < m_globalVVSConvergenceThreshold) { m_vvsConverged = false; - } + } else { m_vvsConverged = true; } -} + } m_robust.MEstimator(vpRobust::TUKEY, m_error, m_weights); @@ -200,7 +200,7 @@ void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &fram } -void vpRBSilhouetteMeTracker::display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const +void vpRBSilhouetteMeTracker::display(const vpCameraParameters &/*cam*/, const vpImage &I, const vpImage &/*IRGB*/, const vpImage &/*depth*/, const vpRBFeatureDisplayType type) const { if (type == vpRBFeatureDisplayType::SIMPLE) { @@ -213,15 +213,15 @@ void vpRBSilhouetteMeTracker::display(const vpCameraParameters &cam, const vpIma // vpDisplay::displayPoint(I, p.icpoint, vpColor::red, 2); } - vpColor cs[6] = { - vpColor::red, - vpColor::blue, - vpColor::green, - vpColor::purple, - vpColor::cyan, - vpColor::darkGreen - - }; + // vpColor cs[6] = { + // vpColor::red, + // vpColor::blue, + // vpColor::green, + // vpColor::purple, + // vpColor::cyan, + // vpColor::darkGreen + + // }; // unsigned colorIndex = 0; // for (const vpTrackedSilhouetteLine &line: m_lines) { // if (line.getPoints().size() > 10) { diff --git a/modules/tracker/rbt/src/mask/vpColorHistogram.cpp b/modules/tracker/rbt/src/mask/vpColorHistogram.cpp index 929a2212aa..77a7c8edbc 100644 --- a/modules/tracker/rbt/src/mask/vpColorHistogram.cpp +++ b/modules/tracker/rbt/src/mask/vpColorHistogram.cpp @@ -55,7 +55,6 @@ void vpColorHistogram::Builder::build(vpColorHistogram &histogram) vpColorHistogram::vpColorHistogram() : m_N(0), m_numPixels(0) { } - vpColorHistogram::vpColorHistogram(unsigned int N) { setBinNumber(N); @@ -187,7 +186,7 @@ void vpColorHistogram::computeSplitHistograms(const vpImage &image, cons std::vector countsIn(bins, 0), countsOut(bins, 0); unsigned binSize = 256 / insideMask.m_N; - unsigned int pixels = 0; + #pragma omp parallel { std::vectorlocalCountsIn(bins, 0), localCountsOut(bins, 0);