Skip to content

Commit

Permalink
cleanup ccd tracker
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamFlt
SamFlt committed Sep 20, 2024
1 parent 13d596b commit 7487929
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 29 deletions.
12 changes: 5 additions & 7 deletions modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,7 +219,7 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker
void onTrackingIterStart() VP_OVERRIDE { }
void onTrackingIterEnd() VP_OVERRIDE { }

double getVVSTrackerWeight() const VP_OVERRIDE { return m_userVvsWeight / (10 * error_ccd.size()); }
double getVVSTrackerWeight() const VP_OVERRIDE { return m_userVvsWeight / (10 * m_error.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 { }
Expand All @@ -228,7 +228,7 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker
void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE;
void updateCovariance(const double /*lambda*/) VP_OVERRIDE
{
m_cov = Sigma_Phi;
m_cov = m_sigma;
}

void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpImage<vpRGBa> &IRGB, const vpImage<unsigned char> &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE;
Expand All @@ -253,20 +253,18 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker
vpCCDParameters m_ccdParameters;

std::vector<vpRBSilhouetteControlPoint> m_controlPoints; //! Silhouette points where to compute CCD statistics
vpImage<unsigned char> m_silhouette;
vpRobust m_robust;

vpCCDStatistics m_stats;
vpCCDStatistics m_prevStats;

vpMatrix Sigma_Phi;
vpMatrix m_sigma;

vpColVector nabla_E; //! Sum of local gradients
vpMatrix hessian_E; //! Sum of local hessians
vpColVector m_gradient; //! Sum of local gradients
vpMatrix m_hessian; //! Sum of local hessians

double m_vvsConvergenceThreshold;
double tol;
vpColVector error_ccd;
std::vector<vpColVector> m_gradients;
std::vector<vpMatrix> m_hessians;
double m_temporalSmoothingFac; //! Smoothing factor used to integrate data from the previous frame.
Expand Down
44 changes: 22 additions & 22 deletions modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &fr
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);
m_sigma = vpMatrix(m_ccdParameters.phi_dim, m_ccdParameters.phi_dim, 0.0);
m_cov.resize(6, 6);
tol = 0.0;
m_vvsConverged = false;
Expand All @@ -121,10 +121,10 @@ void vpRBSilhouetteCCDTracker::initVVS(const vpRBFeatureTrackerInput &/*frame*/,

m_stats.reinit(resolution, normal_points_number);
m_prevStats.reinit(resolution, normal_points_number);
nabla_E = vpMatrix(m_ccdParameters.phi_dim, 1, 0.0);
hessian_E = vpMatrix(m_ccdParameters.phi_dim, m_ccdParameters.phi_dim, 0.0);
m_gradients.resize(m_controlPoints.size() * 2 * normal_points_number, vpColVector(nabla_E.getRows()));
m_hessians.resize(m_controlPoints.size() * 2 * normal_points_number, vpMatrix(hessian_E.getRows(), hessian_E.getCols()));
m_gradient = vpMatrix(m_ccdParameters.phi_dim, 1, 0.0);
m_hessian = vpMatrix(m_ccdParameters.phi_dim, m_ccdParameters.phi_dim, 0.0);
m_gradients.resize(m_controlPoints.size() * 2 * normal_points_number, vpColVector(m_gradient.getRows()));
m_hessians.resize(m_controlPoints.size() * 2 * normal_points_number, vpMatrix(m_hessian.getRows(), m_hessian.getCols()));


//m_weights.resize(nerror_ccd);
Expand Down Expand Up @@ -185,7 +185,7 @@ void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &/*cam*/, const
for (unsigned int i = 0; i < m_controlPoints.size(); ++i) {
double sum = 0.0;
for (unsigned int j = 0; j < nerror_per_point; ++j) {
sum += error_ccd[i * nerror_per_point + j];
sum += m_error[i * nerror_per_point + j];
}
if (sum > maxPointError) {
maxPointError = sum;
Expand Down Expand Up @@ -499,7 +499,7 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
const int npointsccd = m_controlPoints.size();
const int normal_points_number = floor(m_ccdParameters.h / m_ccdParameters.delta_h);
const int nerror_ccd = 2 * normal_points_number * 3 * npointsccd;
error_ccd.resize(nerror_ccd, false);
m_error.resize(nerror_ccd, false);
m_weighted_error.resize(nerror_ccd, false);
m_L.resize(nerror_ccd, 6, false, false);
#ifdef VISP_HAVE_OPENMP
Expand All @@ -526,7 +526,7 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
if (!p.isValid()) {
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;
m_error[i * 2 * normal_points_number * 3 + j * 3 + m] = 0.0;
}
}
continue;
Expand Down Expand Up @@ -555,7 +555,7 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
const double *pix_j = pix_ptr + j * 3;
const double errf = vic_j[4];
const double smooth2 = m_temporalSmoothingFac * m_temporalSmoothingFac;
double *error_ccd_j = error_ccd.data + i * 2 * normal_points_number * 3 + j * 3;
double *error_ccd_j = m_error.data + i * 2 * normal_points_number * 3 + j * 3;

for (unsigned n = 0; n < 9; ++n) {
//double *tmp_cov_ptr = tmp_cov[m];
Expand Down Expand Up @@ -593,13 +593,13 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
}
}

nabla_E = 0.0;
hessian_E = 0.0;
m_gradient = 0.0;
m_hessian = 0.0;
//m_robust.setMinMedianAbsoluteDeviation(1.0);
m_robust.MEstimator(vpRobust::vpRobustEstimatorType::TUKEY, error_ccd, m_weights);
m_robust.MEstimator(vpRobust::vpRobustEstimatorType::TUKEY, m_error, m_weights);

for (unsigned int i = 0; i < m_L.getRows(); ++i) {
m_weighted_error[i] = error_ccd[i] * m_weights[i];
m_weighted_error[i] = m_error[i] * m_weights[i];
for (unsigned int j = 0; j < 6; ++j) {
m_L[i][j] *= m_weights[i];
}
Expand All @@ -612,8 +612,8 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
#pragma omp parallel
#endif
{
vpColVector localGradient(nabla_E.getRows(), 0.0);
vpMatrix localHessian(hessian_E.getRows(), hessian_E.getCols(), 0.0);
vpColVector localGradient(m_gradient.getRows(), 0.0);
vpMatrix localHessian(m_hessian.getRows(), m_hessian.getCols(), 0.0);
#ifdef VISP_HAVE_OPENMP
#pragma omp single
#endif
Expand Down Expand Up @@ -644,16 +644,16 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
localHessians[currentThread] = localHessian;
}
for (unsigned int i = 0; i < localGradients.size(); ++i) {
nabla_E += localGradients[i];
hessian_E += localHessians[i];
m_gradient += localGradients[i];
m_hessian += localHessians[i];
}

m_LTL = hessian_E;
m_LTR = -nabla_E;
m_LTL = m_hessian;
m_LTR = -m_gradient;

vpMatrix hessian_E_inv = hessian_E.inverseByCholesky();
//Sigma_Phi = /*Sigma_Phi +*/ 2*hessian_E_inv;
Sigma_Phi = m_ccdParameters.covarianceIterDecreaseFactor * Sigma_Phi + 2.0 * (1.0 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv;
vpMatrix hessian_E_inv = m_hessian.inverseByCholesky();
//m_sigma = /*m_sigma +*/ 2*hessian_E_inv;
m_sigma = m_ccdParameters.covarianceIterDecreaseFactor * m_sigma + 2.0 * (1.0 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv;

}

Expand Down

0 comments on commit 7487929

Please sign in to comment.