Skip to content

Commit

Permalink
Harmonize usage of render cMo instead of previous_cMo variable
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamFlt
SamFlt committed Dec 17, 2024
1 parent f2d2375 commit 24c4ffd
Show file tree
Hide file tree
Showing 6 changed files with 40 additions and 43 deletions.
53 changes: 22 additions & 31 deletions modules/tracker/rbt/src/core/vpRBTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<vpRBFeatureTracker> &tracker : m_trackers) {
Expand Down Expand Up @@ -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<vpRBFeatureTracker> &tracker : m_trackers) {
Expand Down Expand Up @@ -498,7 +509,7 @@ std::vector<vpRBSilhouettePoint> 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) {
Expand All @@ -523,8 +534,8 @@ std::vector<vpRBSilhouettePoint> vpRBTracker::extractSilhouettePoints(
// if (noNeighbor) {
// points.push_back(vpRBSilhouettePoint(n, m, norm, theta, Z));
// }
}
}
}

return points;
}
Expand All @@ -550,26 +561,6 @@ void vpRBTracker::display(const vpImage<unsigned char> &I, const vpImage<vpRGBa>
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<unsigned int>(bb.getLeft());
// unsigned int stop = linear_index + static_cast<unsigned int>(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<float>(rgb.R), static_cast<float>(rgb.G), static_cast<float>(rgb.B));
// vpRGBf blendF = ((normal + vpRGBf(1.f)) * 127.5f) * blendFactor + rgbF * (1.f - blendFactor);
// IRGB.bitmap[linear_index] = vpRGBa(static_cast<unsigned char>(blendF.R), static_cast<unsigned char>(blendF.G), static_cast<unsigned char>(blendF.B));
// ++linear_index;
// }
// }
vpDisplay::display(IRGB);

for (std::shared_ptr<vpRBFeatureTracker> &tracker : m_trackers) {
if (tracker->featuresShouldBeDisplayed()) {
Expand Down
12 changes: 7 additions & 5 deletions modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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<double>(sp.i) - p.projPrevPx[1], 2) + std::pow(static_cast<double>(sp.j) - p.projPrevPx[0], 2) < vpMath::sqr(5)) {
if (std::pow(static_cast<double>(sp.i) - p.projPrevPx[1], 2) + std::pow(static_cast<double>(sp.j) - p.projPrevPx[0], 2) < vpMath::sqr(3)) {
p.visible = false;
break;
}
Expand Down
3 changes: 2 additions & 1 deletion modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float> &depthMap = frame.depth;
const vpImage<float> &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();
Expand Down
3 changes: 2 additions & 1 deletion modules/tracker/rbt/src/features/vpRBKltTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
9 changes: 5 additions & 4 deletions modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,10 +144,11 @@ template <class T> 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;
Expand Down Expand Up @@ -386,7 +387,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage<vpRGBa> &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;
Expand Down Expand Up @@ -465,7 +466,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage<vpRGBa> &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
Expand Down Expand Up @@ -580,7 +581,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage<vpRGBa> &I,
cov_vic_ptr[9 + m * 3 + m] += m_ccdParameters.kappa;
}
}
}
}

void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
{
Expand Down
3 changes: 2 additions & 1 deletion modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down

0 comments on commit 24c4ffd

Please sign in to comment.