disable tbb when num_threads == 1

koide3 · Sep 22, 2024 · a63ea95 · a63ea95
1 parent afe2874
commit a63ea95
Show file tree

Hide file tree

Showing 12 changed files with 29 additions and 29 deletions.
diff --git a/include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp
@@ -107,7 +107,7 @@ void IntegratedColorConsistencyFactor_<TargetFrame, SourceFrame, IntensityGradie
     correspondences[i] = (num_found && k_sq_dist < max_correspondence_distance_sq) ? k_index : -1;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*source); i++) {
       perpoint_task(i);
@@ -218,7 +218,7 @@ double IntegratedColorConsistencyFactor_<TargetFrame, SourceFrame, IntensityGrad
     bs_source.resize(num_threads, Eigen::Matrix<double, 6, 1>::Zero());
   }
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*source), num_threads, H_target, H_source, H_target_source, b_target, b_source);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*source), H_target, H_source, H_target_source, b_target, b_source);

diff --git a/include/gtsam_points/factors/impl/integrated_colored_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_colored_gicp_factor_impl.hpp
@@ -119,7 +119,7 @@ void IntegratedColoredGICPFactor_<TargetFrame, SourceFrame, IntensityGradients>:
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*source); i++) {
       perpoint_task(i);
@@ -232,7 +232,7 @@ double IntegratedColoredGICPFactor_<TargetFrame, SourceFrame, IntensityGradients
     return error_geom + error_photo;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*source), num_threads, H_target, H_source, H_target_source, b_target, b_source);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*source), H_target, H_source, H_target_source, b_target, b_source);

diff --git a/include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp
@@ -77,7 +77,7 @@ double IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Va
     return error;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*this->source), this->num_threads, nullptr, nullptr, nullptr, nullptr, nullptr);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*this->source), nullptr, nullptr, nullptr, nullptr, nullptr);
@@ -142,7 +142,7 @@ boost::shared_ptr<gtsam::GaussianFactor> IntegratedCT_GICPFactor_<TargetFrame, S
   gtsam::Vector6 b_0;
   gtsam::Vector6 b_1;
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     error = scan_matching_reduce_omp(perpoint_task, frame::size(*this->source), this->num_threads, &H_00, &H_11, &H_01, &b_0, &b_1);
   } else {
     error = scan_matching_reduce_tbb(perpoint_task, frame::size(*this->source), &H_00, &H_11, &H_01, &b_0, &b_1);
@@ -184,7 +184,7 @@ void IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::update_correspondences(
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(this->num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*this->source); i++) {
       perpoint_task(i);

diff --git a/include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp
@@ -104,7 +104,7 @@ double IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Val
     return 0.5 * error * error;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*this->source), this->num_threads, nullptr, nullptr, nullptr, nullptr, nullptr);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*this->source), nullptr, nullptr, nullptr, nullptr, nullptr);
@@ -171,7 +171,7 @@ boost::shared_ptr<gtsam::GaussianFactor> IntegratedCT_ICPFactor_<TargetFrame, So
   gtsam::Vector6 b_0;
   gtsam::Vector6 b_1;
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     error = scan_matching_reduce_omp(perpoint_task, frame::size(*this->source), this->num_threads, &H_00, &H_11, &H_01, &b_0, &b_1);
   } else {
     error = scan_matching_reduce_tbb(perpoint_task, frame::size(*this->source), &H_00, &H_11, &H_01, &b_0, &b_1);
@@ -233,7 +233,7 @@ void IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::update_correspondences()
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(this->num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*this->source); i++) {
       perpoint_task(i);

diff --git a/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp
@@ -140,7 +140,7 @@ void IntegratedGICPFactor_<TargetFrame, SourceFrame>::update_correspondences(con
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*source); i++) {
       perpoint_task(i);
@@ -217,7 +217,7 @@ double IntegratedGICPFactor_<TargetFrame, SourceFrame>::evaluate(
     return error;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*source), num_threads, H_target, H_source, H_target_source, b_target, b_source);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*source), H_target, H_source, H_target_source, b_target, b_source);

diff --git a/include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp
@@ -138,7 +138,7 @@ void IntegratedICPFactor_<TargetFrame, SourceFrame>::update_correspondences(cons
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*source); i++) {
       perpoint_task(i);
@@ -221,7 +221,7 @@ double IntegratedICPFactor_<TargetFrame, SourceFrame>::evaluate(
     return error;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*source), num_threads, H_target, H_source, H_target_source, b_target, b_source);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*source), H_target, H_source, H_target_source, b_target, b_source);

diff --git a/include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp
@@ -85,7 +85,7 @@ void IntegratedPointToPlaneFactor_<TargetFrame, SourceFrame>::update_corresponde
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*source); i++) {
       perpoint_task(i);
@@ -167,7 +167,7 @@ double IntegratedPointToPlaneFactor_<TargetFrame, SourceFrame>::evaluate(
     return error;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*source), num_threads, H_target, H_source, H_target_source, b_target, b_source);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*source), H_target, H_source, H_target_source, b_target, b_source);
@@ -240,7 +240,7 @@ void IntegratedPointToEdgeFactor_<TargetFrame, SourceFrame>::update_corresponden
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*source); i++) {
       perpoint_task(i);
@@ -327,7 +327,7 @@ double IntegratedPointToEdgeFactor_<TargetFrame, SourceFrame>::evaluate(
     return error;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default()|| num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*source), num_threads, H_target, H_source, H_target_source, b_target, b_source);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*source), H_target, H_source, H_target_source, b_target, b_source);

diff --git a/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp
@@ -97,7 +97,7 @@ void IntegratedVGICPFactor_<SourceFrame>::update_correspondences(const Eigen::Is
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame::size(*source); i++) {
       perpoint_task(i);
@@ -178,7 +178,7 @@ double IntegratedVGICPFactor_<SourceFrame>::evaluate(
     return error;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
     return scan_matching_reduce_omp(perpoint_task, frame::size(*source), num_threads, H_target, H_source, H_target_source, b_target, b_source);
   } else {
     return scan_matching_reduce_tbb(perpoint_task, frame::size(*source), H_target, H_source, H_target_source, b_target, b_source);

diff --git a/src/gtsam_points/factors/intensity_gradients.cpp b/src/gtsam_points/factors/intensity_gradients.cpp
@@ -110,7 +110,7 @@ IntensityGradients::Ptr IntensityGradients::estimate(const PointCloud::ConstPtr&
     gradients->intensity_gradients[i] << H.inverse() * e, 0.0;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame->size(); i++) {
       perpoint_task(i);
@@ -219,7 +219,7 @@ IntensityGradients::estimate(const gtsam_points::PointCloudCPU::Ptr& frame, int
     gradients->intensity_gradients[i] << H.inverse() * e, 0.0;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
     for (int i = 0; i < frame->size(); i++) {
       perpoint_task(i);

diff --git a/src/gtsam_points/types/point_cloud_cpu.cpp b/src/gtsam_points/types/point_cloud_cpu.cpp
@@ -425,7 +425,7 @@ PointCloudCPU::Ptr voxelgrid_sampling(const PointCloud::ConstPtr& frame, const d
     return bits;
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 32)
     for (std::int64_t i = 0; i < frame->size(); i++) {
       coord_pt[i] = {calc_coord(i), i};
@@ -522,7 +522,7 @@ PointCloudCPU::Ptr voxelgrid_sampling(const PointCloud::ConstPtr& frame, const d
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 4)
     for (std::int64_t block_begin = 0; block_begin < coord_pt.size(); block_begin += block_size) {
       perblock_task(block_begin);
@@ -608,7 +608,7 @@ randomgrid_sampling(const PointCloud::ConstPtr& frame, const double voxel_resolu
   size_t num_voxels = 0;
   std::vector<std::pair<std::uint64_t, size_t>> coord_pt(frame->size());
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 32)
     for (std::int64_t i = 0; i < frame->size(); i++) {
       coord_pt[i] = {calc_coord(i), i};
@@ -698,7 +698,7 @@ randomgrid_sampling(const PointCloud::ConstPtr& frame, const double voxel_resolu
     std::copy(sub_indices.begin(), sub_indices.end(), indices.begin() + num_points.fetch_add(sub_indices.size()));
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 4)
     for (std::int64_t block_begin = 0; block_begin < coord_pt.size(); block_begin += block_size) {
       perblock_task(block_begin);
@@ -906,7 +906,7 @@ PointCloudCPU::Ptr remove_outliers(const PointCloud::ConstPtr& frame, const int
     std::copy(k_neighbors.begin(), k_neighbors.end(), neighbors.begin() + i * k);
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for schedule(guided, 8) num_threads(num_threads)
     for (int i = 0; i < frame->size(); i++) {
       perpoint_task(i);

diff --git a/src/gtsam_points/util/covariance_estimation.cpp b/src/gtsam_points/util/covariance_estimation.cpp
@@ -54,7 +54,7 @@ std::vector<Eigen::Matrix4d> estimate_covariances(const Eigen::Vector4d* points,
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(params.num_threads) schedule(guided, 8)
     for (int i = 0; i < num_points; i++) {
       perpoint_task(i);

diff --git a/src/gtsam_points/util/normal_estimation.cpp b/src/gtsam_points/util/normal_estimation.cpp
@@ -26,7 +26,7 @@ std::vector<Eigen::Vector4d> estimate_normals(const Eigen::Vector4d* points, con
     }
   };
 
-  if (is_omp_default()) {
+  if (is_omp_default() || num_threads == 1) {
 #pragma omp parallel for num_threads(num_threads)
     for (int i = 0; i < num_points; i++) {
       perpoint_task(i);