From 6affb4bc183c610c362151f682cef489c893e195 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 18 Sep 2024 14:06:35 +0200 Subject: [PATCH] Add guards for openmp usage, add guards for vpRBKltTracker --- .../rbt/include/visp3/rbt/vpRBKltTracker.h | 10 ++++-- .../src/core/vpRBSilhouetteControlPoint.cpp | 32 ++++++++++++++++++- modules/tracker/rbt/src/core/vpRBTracker.cpp | 9 +++++- .../vpRBProbabilistic3DDriftDetector.cpp | 2 ++ .../src/features/vpRBDenseDepthTracker.cpp | 2 ++ .../features/vpRBFeatureTrackerFactory.cpp | 10 +++--- .../rbt/src/features/vpRBKltTracker.cpp | 5 +++ .../src/features/vpRBSilhouetteCCDTracker.cpp | 31 +++++++++++------- .../render-based-tutorial-utils.h | 6 ++++ .../render-based/tutorial-rbt-realsense.cpp | 4 ++- .../render-based/tutorial-rbt-sequence.cpp | 4 ++- 11 files changed, 93 insertions(+), 22 deletions(-) diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h index f312de9033..43ada59a41 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h @@ -37,6 +37,13 @@ #ifndef VP_RB_KLT_TRACKER_H #define VP_RB_KLT_TRACKER_H +#include + +#if (defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)) +#define VP_HAVE_RB_KLT_TRACKER +#endif + +#if defined(VP_HAVE_RB_KLT_TRACKER) #include #include #include @@ -45,8 +52,6 @@ #include - - class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker { public: @@ -195,3 +200,4 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker }; #endif +#endif diff --git a/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp index 6b20e12341..17643452e1 100644 --- a/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp +++ b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp @@ -1,4 +1,34 @@ - +/**************************************************************************** + * + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * +*****************************************************************************/ #include #include diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index 7668cb65d2..cae1431200 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -361,13 +361,20 @@ void vpRBTracker::updateRender(vpRBFeatureTrackerInput &frame) frame.renders.boundingBox = m_renderer.getBoundingBox(); // Extract data from Panda textures -#pragma omp sections +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel sections +#endif + { +#ifdef VISP_HAVE_OPENMP #pragma omp section +#endif { m_renderer.getRenderer()->getRender(frame.renders.normals, frame.renders.depth, frame.renders.boundingBox, m_imageHeight, m_imageWidth); } +#ifdef VISP_HAVE_OPENMP #pragma omp section +#endif { m_renderer.getRenderer()->getRender(frame.renders.silhouetteCanny, frame.renders.isSilhouette, frame.renders.boundingBox, m_imageHeight, m_imageWidth); // m_renderer.placeRenderInto(m_tempRenders.renders.silhouetteCanny, frame.renders.silhouetteCanny, vpRGBf(0.f)); diff --git a/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp index 02eea2ddca..1be086747e 100644 --- a/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp +++ b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp @@ -50,7 +50,9 @@ void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &pre if (m_points.size() > 0) { // Step 0: project all points +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (vpStored3DSurfaceColorPoint &p : m_points) { p.update(cTo, cprevTo, frame.cam); } diff --git a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp index 87cdcb209c..9b004dadeb 100644 --- a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp @@ -125,7 +125,9 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, } double t1 = vpTime::measureTimeMs(); vpRotationMatrix cRo = cMo.getRotationMatrix(); +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (unsigned int i = 0; i < m_depthPoints.size(); ++i) { vpDepthPoint &depthPoint = m_depthPoints[i]; depthPoint.update(cMo, cRo); diff --git a/modules/tracker/rbt/src/features/vpRBFeatureTrackerFactory.cpp b/modules/tracker/rbt/src/features/vpRBFeatureTrackerFactory.cpp index fddcb8ecb7..debe8fa694 100644 --- a/modules/tracker/rbt/src/features/vpRBFeatureTrackerFactory.cpp +++ b/modules/tracker/rbt/src/features/vpRBFeatureTrackerFactory.cpp @@ -54,14 +54,16 @@ vpRBFeatureTrackerFactory::vpRBFeatureTrackerFactory() p->loadJsonConfiguration(j); return p; }); - registerType("klt", [](const nlohmann::json &j) { - std::shared_ptr p(new vpRBKltTracker()); + registerType("depth", [](const nlohmann::json &j) { + std::shared_ptr p(new vpRBDenseDepthTracker()); p->loadJsonConfiguration(j); return p; }); - registerType("depth", [](const nlohmann::json &j) { - std::shared_ptr p(new vpRBDenseDepthTracker()); +#if defined(VP_HAVE_RB_KLT_TRACKER) + registerType("klt", [](const nlohmann::json &j) { + std::shared_ptr p(new vpRBKltTracker()); p->loadJsonConfiguration(j); return p; }); +#endif } diff --git a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp index 8470c3e243..3eea86261e 100644 --- a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp @@ -31,6 +31,9 @@ *****************************************************************************/ #include + +#if defined(VP_HAVE_RB_KLT_TRACKER) + #include #include #include @@ -286,3 +289,5 @@ void vpRBKltTracker::display(const vpCameraParameters &cam, const vpImage + +#ifdef VISP_HAVE_OPENMP #include +#endif #define VISP_DEBUG_CCD_TRACKER 0 @@ -243,7 +246,9 @@ void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &cam, const vpIm void vpRBSilhouetteCCDTracker::updateCCDPoints(const vpHomogeneousMatrix &cMo) { +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (vpRBSilhouetteControlPoint &p : m_controlPoints) { p.updateSilhouettePoint(cMo); } @@ -270,7 +275,9 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, // the second column store the one inside the curve vpMatrix normalized_param = vpMatrix(resolution, 2, 0.0); +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (unsigned int kk = 0; kk < m_controlPoints.size(); kk++) { // temporary points used to store those points in the // normal direction as well as negative normal direction @@ -376,7 +383,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, normalized_param[kk][1] += vic_ptr[10 * negative_normal + 7]; } - } + } #pragma omp parallel for for (unsigned int i = 0; i < resolution; ++i) { @@ -492,7 +499,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, } } -} + } void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() { @@ -503,7 +510,9 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() m_weighted_error.resize(nerror_ccd, false); m_L.resize(nerror_ccd, 6, false, false); double beforeParallel = vpTime::measureTimeMs(); +#ifdef VISP_HAVE_OPENMP #pragma omp parallel +#endif { // vpMatrix tmp_cov(3, 3); @@ -515,7 +524,9 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() double Lnvp[6]; unsigned int normal_points_number = static_cast(floor(m_ccdParameters.h / m_ccdParameters.delta_h)); +#ifdef VISP_HAVE_OPENMP #pragma omp for +#endif for (unsigned int kk = 0; kk < m_controlPoints.size(); kk++) { const int i = kk; const vpRBSilhouetteControlPoint &p = m_controlPoints[kk]; @@ -610,17 +621,22 @@ 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 std::vector localHessians; +#ifdef VISP_HAVE_OPENMP #pragma omp parallel +#endif { vpColVector localGradient(nabla_E.getRows(), 0.0); vpMatrix localHessian(hessian_E.getRows(), hessian_E.getCols(), 0.0); +#ifdef VISP_HAVE_OPENMP #pragma omp single +#endif { localGradients.resize(omp_get_num_threads(), localGradient); localHessians.resize(omp_get_num_threads(), localHessian); } - +#ifdef VISP_HAVE_OPENMP #pragma omp for schedule(static) +#endif for (unsigned int i = 0; i < m_gradients.size(); ++i) { m_gradients[i] *= m_weights[i]; m_hessians[i] *= m_weights[i]; @@ -635,22 +651,13 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() hessian_E += localHessians[i]; } double afterWeight = vpTime::measureTimeMs(); - //std::cout << "Weighting and sum took " << afterWeight - beforeWeightAndSum << std::endl; - - //sigmaF = 0.2*sigmaF + 0.8*computeCovarianceMatrix(m_L,v,error_ccd); - //std::cout << " sigmaF " << sigmaF << std::endl; m_LTL = hessian_E; m_LTR = -nabla_E; - // m_LTL = m_L.AtA(); - // std::cout << m_LTL - hessian_E << std::endl; - // computeJTR(m_L, -m_weighted_error, m_LTR); - vpMatrix hessian_E_inv = hessian_E.inverseByCholesky(); //Sigma_Phi = /*Sigma_Phi +*/ 2*hessian_E_inv; Sigma_Phi = m_ccdParameters.covarianceIterDecreaseFactor * Sigma_Phi + 2 * (1 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv; m_cov = Sigma_Phi; - //std::cout << "Rest took: " << vpTime::measureTimeMs() - afterWeight << std::endl; } diff --git a/tutorial/tracking/render-based/render-based-tutorial-utils.h b/tutorial/tracking/render-based/render-based-tutorial-utils.h index 4f02a8e1b0..a65a09348e 100644 --- a/tutorial/tracking/render-based/render-based-tutorial-utils.h +++ b/tutorial/tracking/render-based/render-based-tutorial-utils.h @@ -136,7 +136,9 @@ class vpRBExperimentLogger vpDisplay::getImage(IRGB, IColOverlay); vpDisplay::getImage(Idepth, IdepthOverlay); vpDisplay::getImage(Imask, ImaskOverlay); +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (unsigned int i = 0; i < IRGB.getHeight(); ++i) { memcpy(Iout[i], IgrayOverlay[i], IRGB.getWidth() * sizeof(vpRGBa)); memcpy(Iout[i] + IRGB.getWidth(), IColOverlay[i], IRGB.getWidth() * sizeof(vpRGBa)); @@ -342,7 +344,9 @@ void enableRendererProfiling() void displayNormals(const vpImage &normalsImage, vpImage &normalDisplayImage) { +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (int i = 0; i < normalsImage.getSize(); ++i) { normalDisplayImage.bitmap[i].R = static_cast((normalsImage.bitmap[i].R + 1.0) * 127.5f); normalDisplayImage.bitmap[i].G = static_cast((normalsImage.bitmap[i].G + 1.0) * 127.5f); @@ -356,7 +360,9 @@ void displayNormals(const vpImage &normalsImage, vpImage &normal void displayCanny(const vpImage &cannyRawData, vpImage &canny, const vpImage &valid) { +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (int i = 0; i < cannyRawData.getSize(); ++i) { vpRGBf &px = cannyRawData.bitmap[i]; canny.bitmap[i] = valid.bitmap[i] * 255; diff --git a/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp b/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp index cac60a4444..c9718ee83c 100644 --- a/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp +++ b/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp @@ -37,7 +37,9 @@ struct CmdArguments void updateDepth(const vpImage &depthRaw, float depthScale, float maxZDisplay, vpImage &depth, vpImage &IdepthDisplay) { depth.resize(depthRaw.getHeight(), depthRaw.getWidth()); +#ifdef VISP_HAVE_OPENMP #pragma omp parallel for +#endif for (unsigned int i = 0; i < depthRaw.getSize(); ++i) { depth.bitmap[i] = depthScale * static_cast(depthRaw.bitmap[i]); IdepthDisplay.bitmap[i] = depth.bitmap[i] > maxZDisplay ? 0 : static_cast((depth.bitmap[i] / maxZDisplay) * 255.f); @@ -53,7 +55,7 @@ int main(int argc, const char **argv) vpRBTrackerTutorial::vpRBExperimentPlotter plotter; vpJsonArgumentParser parser( - "Tutorial showing the usage of the Render-Based tracker with a realsense camera", + "Tutorial showing the usage of the Render-Based tracker with a RealSense camera", "--config", "/" ); diff --git a/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp b/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp index 9e1e440f09..0bf6e2fe30 100644 --- a/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp +++ b/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp @@ -166,7 +166,9 @@ int main(int argc, const char **argv) float scale = 9.999999747378752e-05; depth.resize(dataArray.getHeight(), dataArray.getWidth()); depthDisplay.resize(dataArray.getHeight(), dataArray.getWidth()); -#pragma omp simd +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif for (unsigned int i = 0; i < dataArray.getSize(); ++i) { float value = static_cast(dataArray.bitmap[i]) * scale; depth.bitmap[i] = value;