From dab15b82211754e744ce066c1fbb4065ff48db94 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 20 Sep 2024 18:40:14 +0200 Subject: [PATCH] cleanup silhouette point --- .../visp3/rbt/vpRBSilhouetteCCDTracker.h | 8 +- .../visp3/rbt/vpRBSilhouetteControlPoint.h | 49 +++--- .../src/core/vpRBSilhouetteControlPoint.cpp | 140 +++++++++--------- .../src/features/vpRBSilhouetteCCDTracker.cpp | 7 +- .../src/features/vpRBSilhouetteMeTracker.cpp | 18 +-- 5 files changed, 102 insertions(+), 120 deletions(-) diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h index 3dc4f9e95d..db48267f13 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h @@ -247,7 +247,6 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker protected: void updateCCDPoints(const vpHomogeneousMatrix &cMo); void computeLocalStatistics(const vpImage &I, vpCCDStatistics &stats); - void computeErrorAndInteractionMatrix(); vpCCDParameters m_ccdParameters; @@ -260,15 +259,14 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker vpMatrix m_sigma; - vpColVector m_gradient; //! Sum of local gradients - vpMatrix m_hessian; //! Sum of local hessians - double m_vvsConvergenceThreshold; double tol; + std::vector m_gradients; std::vector m_hessians; + vpColVector m_gradient; //! Sum of local gradients + vpMatrix m_hessian; //! Sum of local hessians double m_temporalSmoothingFac; //! Smoothing factor used to integrate data from the previous frame. - }; END_VISP_NAMESPACE #endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h index b593058f04..ebbd2f917f 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h @@ -69,13 +69,19 @@ class VISP_EXPORT vpRBSilhouetteControlPoint std::vector m_candidates; unsigned int m_numCandidates; - vpMe *me; + const vpMe *m_me; vpMeSite s; + //! Normal to surface where the control point lies + vpColVector norm; + vpColVector normw; + bool m_valid; + bool m_isSilhouette; + + const vpCameraParameters *m_cam; public: - const vpCameraParameters *cam; vpImagePoint icpoint; @@ -83,20 +89,8 @@ class VISP_EXPORT vpRBSilhouetteControlPoint vpPoint cpoint; vpPoint cpointo; - //! Normal to surface where the control point lies - vpColVector norm; - vpColVector normw; - - //! Gradient profile associated to the control Points - - double error; - - vpColVector L; - double xs, ys, nxs, nys, Zs; - bool isSilhouette; - bool invnormal; public: @@ -118,25 +112,22 @@ class VISP_EXPORT vpRBSilhouetteControlPoint void setValid(bool valid) { m_valid = valid; } bool isValid() const { return m_valid; } - int outOfImage(int i, int j, int half, int rows, int cols) const; - int outOfImage(const vpImagePoint &iP, int half, int rows, int cols) const; + + const vpCameraParameters &getCameraParameters() const { return *m_cam; } bool siteIsValid() const { return s.getState() == vpMeSite::NO_SUPPRESSION; } const vpMeSite &getSite() const { return s; } vpMeSite &getSite() { return s; } const vpFeatureLine &getFeatureLine() const { return featureline; } const vpLine &getLine() const { return line; } double getTheta() const { return theta; } - - void setMovingEdge(vpMe *_me) { me = _me; } - void setCameraParameters(const vpCameraParameters *_cam) { cam = _cam; } + bool isSilhouette() const { return m_isSilhouette; } void initControlPoint(const vpImage &I, double cvlt); void detectSilhouette(const vpImage &I); - void buildPoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc); - void buildSilhouettePoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc); - void buildPlane(const vpPoint &pointn, const vpColVector &normal, vpPlane &plane); - void buildPLine(const vpHomogeneousMatrix &oMc); + void buildPoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam, const vpMe &me); + void buildSilhouettePoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam); + void update(const vpHomogeneousMatrix &_cMo); void updateSilhouettePoint(const vpHomogeneousMatrix &_cMo); @@ -156,13 +147,17 @@ class VISP_EXPORT vpRBSilhouetteControlPoint */ void trackMultipleHypotheses(const vpImage &I); - void initInteractionMatrixError(); - void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo); - void computeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo); + void computeMeInteractionMatrixError(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e); + void computeMeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e); private: - void sample(const vpImage &) { } bool isLineDegenerate() const; + + int outOfImage(int i, int j, int half, int rows, int cols) const; + int outOfImage(const vpImagePoint &iP, int half, int rows, int cols) const; + void buildPlane(const vpPoint &pointn, const vpColVector &normal, vpPlane &plane); + void buildPLine(const vpHomogeneousMatrix &oMc); + }; END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp index 41f11628f5..7d3404f31d 100644 --- a/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp +++ b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp @@ -45,15 +45,13 @@ void vpRBSilhouetteControlPoint::init() vpRBSilhouetteControlPoint::vpRBSilhouetteControlPoint() { init(); - me = nullptr; - m_numCandidates = 3; + m_me = nullptr; + m_numCandidates = 1; m_candidates.resize(1); sign = 1; - norm.resize(3); theta = 0; - isSilhouette = false; - invnormal = false; + m_isSilhouette = false; m_valid = true; } @@ -65,24 +63,21 @@ vpRBSilhouetteControlPoint::vpRBSilhouetteControlPoint(const vpRBSilhouetteContr vpRBSilhouetteControlPoint &vpRBSilhouetteControlPoint::operator=(const vpRBSilhouetteControlPoint &meTracker) { - me = meTracker.me; + m_me = meTracker.m_me; s = meTracker.s; m_numCandidates = meTracker.m_numCandidates; - cam = meTracker.cam; + m_cam = meTracker.m_cam; icpoint = meTracker.icpoint; cpoint = meTracker.cpoint; cpointo = meTracker.cpointo; norm = meTracker.norm; normw = meTracker.normw; - error = meTracker.error; - L = meTracker.L; xs = meTracker.xs; ys = meTracker.ys; nxs = meTracker.nxs; nys = meTracker.nys; Zs = meTracker.Zs; - isSilhouette = meTracker.isSilhouette; - invnormal = meTracker.invnormal; + m_isSilhouette = meTracker.m_isSilhouette; rho = meTracker.rho; theta = meTracker.theta; thetaInit = meTracker.thetaInit; @@ -101,25 +96,22 @@ vpRBSilhouetteControlPoint::vpRBSilhouetteControlPoint(const vpRBSilhouetteContr vpRBSilhouetteControlPoint &vpRBSilhouetteControlPoint::operator=(const vpRBSilhouetteControlPoint &&meTracker) { - me = std::move(meTracker.me); + m_me = std::move(meTracker.m_me); s = std::move(meTracker.s); thetaInit = std::move(meTracker.thetaInit); m_numCandidates = std::move(meTracker.m_numCandidates); - cam = std::move(meTracker.cam); + m_cam = std::move(meTracker.m_cam); icpoint = std::move(meTracker.icpoint); cpoint = std::move(meTracker.cpoint); cpointo = std::move(meTracker.cpointo); norm = std::move(meTracker.norm); normw = std::move(meTracker.normw); - error = std::move(meTracker.error); - L = std::move(meTracker.L); xs = std::move(meTracker.xs); ys = std::move(meTracker.ys); nxs = std::move(meTracker.nxs); nys = std::move(meTracker.nys); Zs = std::move(meTracker.Zs); - isSilhouette = std::move(meTracker.isSilhouette); - invnormal = std::move(meTracker.invnormal); + m_isSilhouette = std::move(meTracker.m_isSilhouette); rho = std::move(meTracker.rho); theta = std::move(meTracker.theta); delta = std::move(meTracker.delta); @@ -156,10 +148,10 @@ void vpRBSilhouetteControlPoint::track(const vpImage &I) if (s.getState() == vpMeSite::NO_SUPPRESSION) { try { if (s.m_convlt == 0) { - s.track(I, me, false); + s.track(I, m_me, false); } else { - s.track(I, me, false); + s.track(I, m_me, false); } } catch (vpTrackingException &) { @@ -175,7 +167,7 @@ void vpRBSilhouetteControlPoint::trackMultipleHypotheses(const vpImageget_px(); - double py = cam->get_py(); - int jc = cam->get_u0(); - int ic = cam->get_v0(); + double px = m_cam->get_px(); + double py = m_cam->get_py(); + int jc = m_cam->get_u0(); + int ic = m_cam->get_v0(); icpoint.set_i(n); icpoint.set_j(m); double x, y; @@ -260,16 +255,17 @@ vpRBSilhouetteControlPoint::buildPoint(int n, int m, const double &Z, double ori void vpRBSilhouetteControlPoint::buildSilhouettePoint(int n, int m, const double &Z, double orient, const vpColVector &normo, - const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc) + const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam) { + m_cam = &cam; vpRotationMatrix R; cMo.extract(R); theta = orient; thetaInit = theta; - double px = cam->get_px(); - double py = cam->get_py(); - int jc = cam->get_u0(); - int ic = cam->get_v0(); + double px = m_cam->get_px(); + double py = m_cam->get_py(); + int jc = m_cam->get_u0(); + int ic = m_cam->get_v0(); icpoint.set_i(n); icpoint.set_j(m); xs = (m-jc)/px; @@ -281,11 +277,11 @@ vpRBSilhouetteControlPoint::buildSilhouettePoint(int n, int m, const double &Z, double x, y; x = (m-jc)/px; y = (n-ic)/py; - cpoint.setWorldCoordinates(x*Z, y*Z, Z); + cpoint.setWorldCoordinates(x * Z, y * Z, Z); cpoint.changeFrame(oMc); cpointo.setWorldCoordinates(cpoint.get_X(), cpoint.get_Y(), cpoint.get_Z()); normw = normo; - norm = R*normo; + norm = R * normo; buildPLine(oMc); #if VISP_DEBUG_RB_CONTROL_POINT if (std::isnan(line.getTheta())) { @@ -302,10 +298,10 @@ vpRBSilhouetteControlPoint::update(const vpHomogeneousMatrix &_cMo) { cpointo.changeFrame(_cMo); cpointo.projection(); - double px = cam->get_px(); - double py = cam->get_py(); - double uc = cam->get_u0(); - double vc = cam->get_v0(); + double px = m_cam->get_px(); + double py = m_cam->get_py(); + double uc = m_cam->get_u0(); + double vc = m_cam->get_v0(); double u, v; v = py*cpointo.get_y()+vc; u = px*cpointo.get_x()+uc; @@ -317,10 +313,10 @@ vpRBSilhouetteControlPoint::updateSilhouettePoint(const vpHomogeneousMatrix &cMo { cpointo.changeFrame(cMo); cpointo.projection(); - const double px = cam->get_px(); - const double py = cam->get_py(); - const double uc = cam->get_u0(); - const double vc = cam->get_v0(); + const double px = m_cam->get_px(); + const double py = m_cam->get_py(); + const double uc = m_cam->get_u0(); + const double vc = m_cam->get_v0(); const double v = py * cpointo.get_y() + vc; const double u = px * cpointo.get_x() + uc; icpoint.set_uv(u, v); @@ -360,12 +356,12 @@ void vpRBSilhouetteControlPoint::initControlPoint(const vpImage & { double delta = theta; s.init((double)icpoint.get_i(), (double)icpoint.get_j(), delta, cvlt, sign); - if (me != nullptr) { - const double marginRatio = me->getThresholdMarginRatio(); - const double convolution = s.convolution(I, me); + if (m_me != nullptr) { + const double marginRatio = m_me->getThresholdMarginRatio(); + const double convolution = s.convolution(I, m_me); s.init((double)icpoint.get_i(), (double)icpoint.get_j(), delta, convolution, sign); const double contrastThreshold = fabs(convolution) * marginRatio; - s.setContrastThreshold(contrastThreshold, *me); + s.setContrastThreshold(contrastThreshold, *m_me); } } @@ -382,24 +378,15 @@ void vpRBSilhouetteControlPoint::detectSilhouette(const vpImage &I) unsigned int isBg = static_cast(I[ii][jj] == 0.f); k += isBg; } - isSilhouette = k > 2; + m_isSilhouette = k > 2; } -/*! - Initialize the interaction matrix and the error to 0. -*/ -void -vpRBSilhouetteControlPoint::initInteractionMatrixError() -{ - L.resize(6, false); - error = 0; -} /*! Compute the interaction matrix and the error vector corresponding to the line. */ void -vpRBSilhouetteControlPoint::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo) +vpRBSilhouetteControlPoint::computeMeInteractionMatrixError(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e) { line.changeFrame(cMo); @@ -421,10 +408,10 @@ vpRBSilhouetteControlPoint::computeInteractionMatrixError(const vpHomogeneousMat double co = cos(theta0); double si = sin(theta0); - double mx = 1.0/cam->get_px(); - double my = 1.0/cam->get_py(); - double xc = cam->get_u0(); - double yc = cam->get_v0(); + double mx = 1.0 / m_cam->get_px(); + double my = 1.0 / m_cam->get_py(); + double xc = m_cam->get_u0(); + double yc = m_cam->get_v0(); vpMatrix H; H = featureline.interaction(); @@ -439,15 +426,22 @@ vpRBSilhouetteControlPoint::computeInteractionMatrixError(const vpHomogeneousMat double *Ltheta = H[1]; // Calculate interaction matrix for a distance for (unsigned int k = 0; k < 6; k++) { - L[k] = (Lrho[k] + alpha*Ltheta[k]); + L[i][k] = (Lrho[k] + alpha*Ltheta[k]); } - error = rho0 - (x*co + y*si); + e[i] = rho0 - (x*co + y*si); m_valid = true; } + else { + m_valid = false; + e[i] = 0; + for (unsigned int k = 0; k < 6; k++) { + L[i][k] = 0.0; + } + } } void -vpRBSilhouetteControlPoint::computeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo) +vpRBSilhouetteControlPoint::computeMeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e) { line.changeFrame(cMo); @@ -458,17 +452,19 @@ vpRBSilhouetteControlPoint::computeInteractionMatrixErrorMH(const vpHomogeneousM vpFeatureBuilder::create(featureline, line); const double rho0 = featureline.getRho(); const double theta0 = featureline.getTheta(); +#if VISP_DEBUG_RB_CONTROL_POINT if (std::isnan(theta0)) { - throw; + throw vpException(vpException::fatalError, "Got nan theta in computeInteractionMatrixMH"); } +#endif const double co = cos(theta0); const double si = sin(theta0); - const double mx = 1.0/cam->get_px(); - const double my = 1.0/cam->get_py(); - const double xc = cam->get_u0(); - const double yc = cam->get_v0(); + const double mx = 1.0 / m_cam->get_px(); + const double my = 1.0 / m_cam->get_py(); + const double xc = m_cam->get_u0(); + const double yc = m_cam->get_v0(); const vpMatrix &H = featureline.interaction(); double xmin, ymin; double errormin = std::numeric_limits::max(); @@ -493,14 +489,20 @@ vpRBSilhouetteControlPoint::computeInteractionMatrixErrorMH(const vpHomogeneousM } } if (m_valid) { - error = rho0 - (xmin * co + ymin * si); + e[i] = rho0 - (xmin * co + ymin * si); const double alpha = xmin * si - ymin * co; const double *Lrho = H[0]; const double *Ltheta = H[1]; // Calculate interaction matrix for a distance for (unsigned int k = 0; k < 6; k++) { - L[k] = (Lrho[k] + alpha * Ltheta[k]); + L[i][k] = (Lrho[k] + alpha * Ltheta[k]); + } + } + else { + e[i] = 0; + for (unsigned int k = 0; k < 6; k++) { + L[i][k] = 0.0; } } } diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp index 39e9dd0ea1..d1cbdb5fea 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp @@ -95,10 +95,9 @@ void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &fr int ii = sp.i, jj = sp.j; vpRBSilhouetteControlPoint pccd; - pccd.setCameraParameters(&frame.cam); - pccd.buildSilhouettePoint(ii, jj, sp.Z, sp.orientation, sp.normal, cMo, oMc); + pccd.buildSilhouettePoint(ii, jj, sp.Z, sp.orientation, sp.normal, cMo, oMc, frame.cam); pccd.detectSilhouette(frame.renders.depth); - if (pccd.isSilhouette && !std::isnan(sp.orientation) && pccd.isValid()) { + if (pccd.isSilhouette() && !std::isnan(sp.orientation) && pccd.isValid()) { m_controlPoints.push_back(std::move(pccd)); } } @@ -540,7 +539,7 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() const double *mean_vic_ptr_prev = m_prevStats.mean_vic[i]; const double *cov_vic_ptr_prev = m_prevStats.cov_vic[i]; - const vpCameraParameters &cam = *p.cam; + const vpCameraParameters &cam = p.getCameraParameters(); Lnvp[0] = (-nv_ptr[0] / p.Zs); Lnvp[1] = (-nv_ptr[1] / p.Zs); diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp index 7833e7228c..6be651021c 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp @@ -68,9 +68,7 @@ void vpRBSilhouetteMeTracker::extractFeatures(const vpRBFeatureTrackerInput &fra } vpRBSilhouetteControlPoint p; - p.setCameraParameters(&frame.cam); - p.setMovingEdge(&m_me); - p.buildPoint((int)sp.i, (int)sp.j, sp.Z, sp.orientation, sp.normal, cMo, oMc); + p.buildPoint((int)sp.i, (int)sp.j, sp.Z, sp.orientation, sp.normal, cMo, oMc, frame.cam, m_me); if (previousFrame.I.getSize() == frame.I.getSize()) { p.initControlPoint(previousFrame.I, 0); } @@ -107,10 +105,6 @@ void vpRBSilhouetteMeTracker::initVVS(const vpRBFeatureTrackerInput & /*frame*/, return; } - for (unsigned int k = 0; k < m_controlPoints.size(); k++) { - m_controlPoints[k].initInteractionMatrixError(); - } - m_weighted_error.resize(m_numFeatures, false); m_weights.resize(m_numFeatures, false); m_weights = 0; @@ -132,15 +126,12 @@ void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &fram vpRBSilhouetteControlPoint &p = m_controlPoints[k]; //p.update(cMo); if (m_numCandidates <= 1) { - p.computeInteractionMatrixError(cMo); + p.computeMeInteractionMatrixError(cMo, k, m_L, m_error); } else { - p.computeInteractionMatrixErrorMH(cMo); + p.computeMeInteractionMatrixErrorMH(cMo, k, m_L, m_error); } - - m_error[k] = p.error; //On remplit la matrice d'erreur - m_weights[k] = 1; if (!p.siteIsValid() || !p.isValid()) { factor[k] = 0.0; @@ -153,9 +144,6 @@ void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &fram if (m_error[k] <= threshold) { ++count; } - for (unsigned int j = 0; j < 6; j++) { - m_L[k][j] = p.L[j]; - } } }