diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index c159d27921..f70cb421dd 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -245,7 +245,25 @@ void vpRBTracker::track(vpRBFeatureTrackerInput &input) throw vpException(vpException::badValue, "Could not extract silhouette from depth canny: Object may not be in image"); } } - m_logger.setSilhouetteTime(m_logger.endTimer()); + + if (m_odometry) { + m_logger.startTimer(); + m_odometry->compute(input, m_previousFrame); + vpHomogeneousMatrix cnTc = m_odometry->getCameraMotion(); + m_cMo = cnTc * m_cMo; + updateRender(input); + m_logger.setOdometryTime(m_logger.endTimer()); + } + + if (requiresSilhouetteCandidates) { + const vpHomogeneousMatrix cTcp = m_cMo * m_cMoPrev.inverse(); + input.silhouettePoints = extractSilhouettePoints(input.renders.normals, input.renders.depth, + input.renders.silhouetteCanny, input.renders.isSilhouette, input.cam, cTcp); + if (input.silhouettePoints.size() == 0) { + throw vpException(vpException::badValue, "Could not extract silhouette from depth canny: Object may not be in image"); + } + } + int id = 0; for (std::shared_ptr &tracker : m_trackers) { @@ -281,14 +299,7 @@ void vpRBTracker::track(vpRBFeatureTrackerInput &input) id += 1; } - if (m_odometry) { - m_logger.startTimer(); - m_odometry->compute(input, m_previousFrame); - vpHomogeneousMatrix cnTc = m_odometry->getCameraMotion(); - m_cMo = cnTc * m_cMo; - updateRender(input); - m_logger.setOdometryTime(m_logger.endTimer()); - } + id = 0; for (std::shared_ptr &tracker : m_trackers) { @@ -498,7 +509,7 @@ std::vector vpRBTracker::extractSilhouettePoints( #if defined(VISP_DEBUG_RB_TRACKER) if (fabs(theta) > M_PI + 1e-6) { throw vpException(vpException::badValue, "Theta expected to be in -Pi, Pi range but was not"); - } + } #endif points.push_back(vpRBSilhouettePoint(n, m, norm, theta, Z)); // if (Zn > 0) { @@ -523,8 +534,8 @@ std::vector vpRBTracker::extractSilhouettePoints( // if (noNeighbor) { // points.push_back(vpRBSilhouettePoint(n, m, norm, theta, Z)); // } + } } -} return points; } @@ -550,26 +561,6 @@ void vpRBTracker::display(const vpImage &I, const vpImage return; } - // vpRect bb = m_currentFrame.boundingBox; - // unsigned int bottom = bb.getBottom(); - // for (unsigned int i = bb.getTop(); i < bottom; ++i) { - // unsigned int linear_index = i * IRGB.getWidth() + static_cast(bb.getLeft()); - // unsigned int stop = linear_index + static_cast(bb.getWidth()); - // while (linear_index < stop) { - // const vpRGBf &normal = m_currentFrame.renders.normals.bitmap[linear_index]; - // if (normal.R == 0.f && normal.G == 0.f && normal.B == 0.f) { - // ++linear_index; - // continue; - // } - // const vpRGBa rgb = IRGB.bitmap[linear_index]; - // const float blendFactor = 0.5; - // const vpRGBf rgbF = vpRGBf(static_cast(rgb.R), static_cast(rgb.G), static_cast(rgb.B)); - // vpRGBf blendF = ((normal + vpRGBf(1.f)) * 127.5f) * blendFactor + rgbF * (1.f - blendFactor); - // IRGB.bitmap[linear_index] = vpRGBa(static_cast(blendF.R), static_cast(blendF.G), static_cast(blendF.B)); - // ++linear_index; - // } - // } - vpDisplay::display(IRGB); for (std::shared_ptr &tracker : m_trackers) { if (tracker->featuresShouldBeDisplayed()) { diff --git a/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp index 258f2562c2..65cb50fae5 100644 --- a/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp +++ b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp @@ -44,8 +44,9 @@ BEGIN_VISP_NAMESPACE void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, - const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) + const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &/*cprevTo*/) { + const vpHomogeneousMatrix &cprevTo = frame.renders.cMo; const vpTranslationVector t = cprevTo.inverse().getTranslationVector(); if (m_points.size() > 0) { @@ -83,18 +84,19 @@ void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &pre vpRGBf normalObject = frame.renders.normals[p.projPrevPx[1]][p.projPrevPx[0]]; - vpColVector vector({ t[0] - p.X[0], t[1] - p.X[1], t[2] - p.X[2] }); + vpColVector cameraRay({ t[0] - p.X[0], t[1] - p.X[1], t[2] - p.X[2] }); - vector.normalize(); - double angle = acos(vpColVector::dotProd(vpColVector({ normalObject.R, normalObject.G, normalObject.B }).normalize(), vector)); + cameraRay.normalize(); + double angle = acos(vpColVector::dotProd(vpColVector({ normalObject.R, normalObject.G, normalObject.B }).normalize(), cameraRay)); if (angle > vpMath::rad(75)) { p.visible = false; continue; } + // Filter points that are too close to the silhouette edges if (frame.silhouettePoints.size() > 0) { for (const vpRBSilhouettePoint &sp: frame.silhouettePoints) { - if (std::pow(static_cast(sp.i) - p.projPrevPx[1], 2) + std::pow(static_cast(sp.j) - p.projPrevPx[0], 2) < vpMath::sqr(5)) { + if (std::pow(static_cast(sp.i) - p.projPrevPx[1], 2) + std::pow(static_cast(sp.j) - p.projPrevPx[0], 2) < vpMath::sqr(3)) { p.visible = false; break; } diff --git a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp index 67b0617190..ecfee46fce 100644 --- a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp @@ -52,11 +52,12 @@ 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*/) { const vpImage &depthMap = frame.depth; const vpImage &renderDepth = frame.renders.depth; vpRect bb = frame.renders.boundingBox; + const vpHomogeneousMatrix &cMo = frame.renders.cMo; vpHomogeneousMatrix oMc = cMo.inverse(); vpRotationMatrix cRo = cMo.getRotationMatrix(); bool useMask = m_useMask && frame.hasMask(); diff --git a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp index 4f68172573..b86a206027 100644 --- a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp @@ -79,10 +79,11 @@ vpRBKltTracker::vpRBKltTracker() : m_maxErrorOutliersPixels(10.0), m_useMask(false), m_minMaskConfidence(0.0) { } -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 &cMo = frame.renders.cMo; const vpHomogeneousMatrix oMc = cMo.inverse(); if (m_maxErrorOutliersPixels > 0.0) { const double distanceThresholdPxSquare = vpMath::sqr(m_maxErrorOutliersPixels); diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp index 453c7f056a..1729a3fe4d 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp @@ -144,10 +144,11 @@ template class FastVec3 vpRBSilhouetteCCDTracker::vpRBSilhouetteCCDTracker() : vpRBFeatureTracker(), m_vvsConvergenceThreshold(0.0), m_temporalSmoothingFac(0.1), m_useMask(false), m_minMaskConfidence(0.0) { } -void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &cMo) +void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) { m_controlPoints.clear(); //m_controlPoints.reserve(frame.silhouettePoints.size()); + const vpHomogeneousMatrix cMo = frame.renders.cMo; const vpHomogeneousMatrix oMc = cMo.inverse(); for (const vpRBSilhouettePoint &sp : frame.silhouettePoints) { // std::cout << m_ccdParameters.h << std::endl; @@ -386,7 +387,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, #if VISP_DEBUG_CCD_TRACKER if (std::isnan(nv_ptr[0]) || std::isnan(nv_ptr[1])) { throw vpException(vpException::fatalError, "x: %f, theta = %f", p.xs, p.getTheta()); - } + } #endif int k = 0; @@ -465,7 +466,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, vic_ptr[10 * negative_normal + 9] = exp(-dist2[0] * dist2[0] / (2 * sigma * sigma)) / (sqrt(2 * CV_PI) * sigma); normalized_param[kk][1] += vic_ptr[10 * negative_normal + 7]; } - } +} #ifdef VISP_HAVE_OPENMP #pragma omp parallel for @@ -580,7 +581,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, cov_vic_ptr[9 + m * 3 + m] += m_ccdParameters.kappa; } } - } +} void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() { diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp index 175503f5a9..e9872a95c3 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp @@ -37,10 +37,11 @@ BEGIN_VISP_NAMESPACE /** * @brief Extract the geometric features from the list of collected silhouette points */ -void vpRBSilhouetteMeTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) +void vpRBSilhouetteMeTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &/*cMo*/) { m_controlPoints.clear(); m_controlPoints.reserve(frame.silhouettePoints.size()); + const vpHomogeneousMatrix &cMo = frame.renders.cMo; const vpHomogeneousMatrix oMc = cMo.inverse(); vpColVector oC = oMc.getRotationMatrix() * vpColVector({ 0.0, 0.0, -1.0 }); for (const vpRBSilhouettePoint &sp: frame.silhouettePoints) {