From ca90f0fd7d431bf77d3650fbd7ae6bb38848023d Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 16 Jul 2024 17:59:59 +0900 Subject: [PATCH 1/9] refactor based on linter Signed-off-by: a-maumau --- apps/align.cpp | 12 +- apps/check_covariance.cpp | 19 +- apps/pcd_map_grid_manager.hpp | 12 +- apps/regression_test.cpp | 4 +- apps/timer.hpp | 6 +- apps/util.hpp | 27 +- .../estimate_covariance.hpp | 14 +- .../multi_voxel_grid_covariance_omp.h | 8 +- .../multi_voxel_grid_covariance_omp_impl.hpp | 17 +- include/multigrid_pclomp/multigrid_ndt_omp.h | 9 +- .../multigrid_ndt_omp_impl.hpp | 94 +++--- include/pclomp/gicp_omp.h | 2 +- include/pclomp/gicp_omp_impl.hpp | 21 +- include/pclomp/ndt_omp.h | 4 +- include/pclomp/ndt_omp_impl.hpp | 280 ++++++++++-------- include/pclomp/ndt_struct.hpp | 14 +- include/pclomp/voxel_grid_covariance_omp.h | 4 +- .../pclomp/voxel_grid_covariance_omp_impl.hpp | 45 +-- .../estimate_covariance.cpp | 16 +- 19 files changed, 345 insertions(+), 263 deletions(-) diff --git a/apps/align.cpp b/apps/align.cpp index f368b43..295498a 100644 --- a/apps/align.cpp +++ b/apps/align.cpp @@ -25,13 +25,13 @@ pcl::PointCloud::Ptr align( auto t1 = std::chrono::system_clock::now(); registration->align(*aligned); auto t2 = std::chrono::system_clock::now(); - std::cout << "single : " << (t2 - t1).count() / 1e6 << "[msec]" << std::endl; + std::cout << "single : " << static_cast((t2 - t1).count()) / 1e6 << "[msec]" << std::endl; for (int i = 0; i < 10; i++) { registration->align(*aligned); } auto t3 = std::chrono::system_clock::now(); - std::cout << "10times: " << (t3 - t2).count() / 1e6 << "[msec]" << std::endl; + std::cout << "10times: " << static_cast((t3 - t2).count()) / 1e6 << "[msec]" << std::endl; std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl; return aligned; @@ -73,11 +73,13 @@ int main(int argc, char ** argv) voxelgrid.filter(*downsampled); source_cloud = downsampled; + pcl::PointCloud::Ptr aligned; + // benchmark std::cout << "--- pcl::GICP ---" << std::endl; pcl::GeneralizedIterativeClosestPoint::Ptr gicp( new pcl::GeneralizedIterativeClosestPoint()); - pcl::PointCloud::Ptr aligned = align(gicp, target_cloud, source_cloud); + align(gicp, target_cloud, source_cloud); std::cout << "--- pclomp::GICP ---" << std::endl; pclomp::GeneralizedIterativeClosestPoint::Ptr gicp_omp( @@ -88,7 +90,7 @@ int main(int argc, char ** argv) pcl::NormalDistributionsTransform::Ptr ndt( new pcl::NormalDistributionsTransform()); ndt->setResolution(1.0); - aligned = align(ndt, target_cloud, source_cloud); + align(ndt, target_cloud, source_cloud); std::vector num_threads = {1, omp_get_max_threads()}; std::vector> search_methods = { @@ -108,7 +110,7 @@ int main(int argc, char ** argv) << std::endl; ndt_omp->setNumThreads(n); ndt_omp->setNeighborhoodSearchMethod(search_method.second); - aligned = align(ndt_omp, target_cloud, source_cloud); + align(ndt_omp, target_cloud, source_cloud); } std::cout << "--- multigrid_pclomp::NDT (" << n << " threads) ---" << std::endl; diff --git a/apps/check_covariance.cpp b/apps/check_covariance.cpp index 251578a..67e51a1 100644 --- a/apps/check_covariance.cpp +++ b/apps/check_covariance.cpp @@ -47,7 +47,7 @@ int main(int argc, char ** argv) std::cerr << "initial_pose_list.size() != source_pcd_list.size()" << std::endl; return 1; } - const int64_t n_data = initial_pose_list.size(); + const auto n_data = static_cast(initial_pose_list.size()); // prepare ndt std::shared_ptr> @@ -93,7 +93,7 @@ int main(int argc, char ** argv) // execute align for (int64_t i = 0; i < n_data; i++) { - const Eigen::Matrix4f initial_pose = initial_pose_list[i]; + const Eigen::Matrix4f & initial_pose = initial_pose_list[i]; const std::string & source_pcd = source_pcd_list[i]; pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud()); if (pcl::io::loadPCDFile(source_pcd, *source_cloud)) { @@ -115,10 +115,11 @@ int main(int argc, char ** argv) // (1) Laplace approximation t1 = std::chrono::system_clock::now(); const Eigen::Matrix2d cov_by_la = - pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian); + pclomp::estimate_xy_covariance_by_laplace_approximation(ndt_result.hessian); t2 = std::chrono::system_clock::now(); const auto elapsed_la = - std::chrono::duration_cast(t2 - t1).count() / 1000.0; + static_cast(std::chrono::duration_cast(t2 - t1).count()) / + 1000.0; // (2) Multi NDT t1 = std::chrono::system_clock::now(); @@ -128,7 +129,8 @@ int main(int argc, char ** argv) result_of_mndt.covariance, ndt_result.pose, 0.0225, 0.0225); t2 = std::chrono::system_clock::now(); const auto elapsed_mndt = - std::chrono::duration_cast(t2 - t1).count() / 1000.0; + static_cast(std::chrono::duration_cast(t2 - t1).count()) / + 1000.0; // (3) Multi NDT with score const double temperature = 0.1; @@ -140,7 +142,8 @@ int main(int argc, char ** argv) result_of_mndt_score.covariance, ndt_result.pose, 0.0225, 0.0225); t2 = std::chrono::system_clock::now(); const auto elapsed_mndt_score = - std::chrono::duration_cast(t2 - t1).count() / 1000.0; + static_cast(std::chrono::duration_cast(t2 - t1).count()) / + 1000.0; // output result const auto result_x = ndt_result.pose(0, 3); @@ -174,7 +177,7 @@ int main(int argc, char ** argv) // output multi ndt result std::ofstream ofs_mndt(multi_ndt_dir + "/" + filename_ss.str()); - const int n_mndt = result_of_mndt.ndt_results.size(); + const auto n_mndt = static_cast(result_of_mndt.ndt_results.size()); ofs_mndt << "index,score,initial_x,initial_y,result_x,result_y" << std::endl; ofs_mndt << std::fixed; for (int j = 0; j < n_mndt; j++) { @@ -190,7 +193,7 @@ int main(int argc, char ** argv) // output multi ndt score result std::ofstream ofs_mndt_score(multi_ndt_score_dir + "/" + filename_ss.str()); - const int n_mndt_score = result_of_mndt_score.ndt_results.size(); + const auto n_mndt_score = static_cast(result_of_mndt_score.ndt_results.size()); ofs_mndt_score << "index,score,initial_x,initial_y,result_x,result_y" << std::endl; ofs_mndt_score << std::fixed; for (int j = 0; j < n_mndt_score; j++) { diff --git a/apps/pcd_map_grid_manager.hpp b/apps/pcd_map_grid_manager.hpp index b64a05c..a58d0a8 100644 --- a/apps/pcd_map_grid_manager.hpp +++ b/apps/pcd_map_grid_manager.hpp @@ -16,6 +16,12 @@ #define NDT_OMP__APPS__PCD_MAP_GRID_MANAGER_HPP_ #include +#include + +#include +#include +#include +#include using AddPair = std::pair::Ptr>; using AddResult = std::vector; @@ -24,7 +30,7 @@ using RemoveResult = std::vector; class MapGridManager { public: - MapGridManager(const pcl::PointCloud::Ptr target_cloud) + explicit MapGridManager(const pcl::PointCloud::Ptr target_cloud) : target_cloud_(target_cloud) { for (const auto & point : target_cloud_->points) { @@ -53,7 +59,7 @@ class MapGridManager if (map_grid_.count(std::make_pair(x, y)) == 0) { continue; } - curr_keys.push_back(std::make_pair(x, y)); + curr_keys.emplace_back(std::make_pair(x, y)); } } @@ -84,7 +90,7 @@ class MapGridManager std::map, pcl::PointCloud::Ptr> map_grid_; std::vector> held_keys_; - std::string to_string_key(const std::pair & key) + static std::string to_string_key(const std::pair & key) { return std::to_string(key.first) + "_" + std::to_string(key.second); } diff --git a/apps/regression_test.cpp b/apps/regression_test.cpp index b2c9614..8800a77 100644 --- a/apps/regression_test.cpp +++ b/apps/regression_test.cpp @@ -62,7 +62,7 @@ int main(int argc, char ** argv) std::cerr << "initial_pose_list.size() != source_pcd_list.size()" << std::endl; return 1; } - const int64_t n_data = initial_pose_list.size(); + const auto n_data = static_cast(initial_pose_list.size()); // prepare ndt pclomp::MultiGridNormalDistributionsTransform::Ptr mg_ndt_omp( @@ -89,7 +89,7 @@ int main(int argc, char ** argv) // execute align for (int64_t i = 0; i < n_data; i++) { // get input - const Eigen::Matrix4f initial_pose = initial_pose_list[i]; + const Eigen::Matrix4f & initial_pose = initial_pose_list[i]; const std::string & source_pcd = source_pcd_list[i]; const pcl::PointCloud::Ptr source_cloud = load_pcd(source_pcd); mg_ndt_omp->setInputSource(source_cloud); diff --git a/apps/timer.hpp b/apps/timer.hpp index 086b1c8..d0972df 100644 --- a/apps/timer.hpp +++ b/apps/timer.hpp @@ -24,12 +24,12 @@ class Timer public: void start() { start_time_ = std::chrono::steady_clock::now(); } - double elapsed_milliseconds() const + [[nodiscard]] double elapsed_milliseconds() const { const auto end_time = std::chrono::steady_clock::now(); const auto elapsed_time = end_time - start_time_; - const double microseconds = - std::chrono::duration_cast(elapsed_time).count(); + const double microseconds = static_cast( + std::chrono::duration_cast(elapsed_time).count()); return microseconds / 1000.0; } diff --git a/apps/util.hpp b/apps/util.hpp index 56a364c..8f63fde 100644 --- a/apps/util.hpp +++ b/apps/util.hpp @@ -15,20 +15,29 @@ #ifndef NDT_OMP__APPS__UTIL_HPP_ #define NDT_OMP__APPS__UTIL_HPP_ -std::vector glob(const std::string & input_dir) +#include +#include +#include +#include + +#include +#include +#include + +inline std::vector glob(const std::string & input_dir) { glob_t buffer; std::vector files; - glob((input_dir + "/*").c_str(), 0, NULL, &buffer); + glob((input_dir + "/*").c_str(), 0, nullptr, &buffer); for (size_t i = 0; i < buffer.gl_pathc; i++) { - files.push_back(buffer.gl_pathv[i]); + files.emplace_back(buffer.gl_pathv[i]); } globfree(&buffer); std::sort(files.begin(), files.end()); return files; } -pcl::PointCloud::Ptr load_pcd(const std::string & path) +inline pcl::PointCloud::Ptr load_pcd(const std::string & path) { pcl::PointCloud::Ptr pcd(new pcl::PointCloud()); if (pcl::io::loadPCDFile(path, *pcd)) { @@ -38,7 +47,7 @@ pcl::PointCloud::Ptr load_pcd(const std::string & path) return pcd; } -std::vector load_pose_list(const std::string & path) +inline std::vector load_pose_list(const std::string & path) { /* timestamp,pose_x,pose_y,pose_z,quat_w,quat_x,quat_y,quat_z,twist_linear_x,twist_linear_y,twist_linear_z,twist_angular_x,twist_angular_y,twist_angular_z @@ -66,8 +75,12 @@ std::vector load_pose_list(const std::string & path) const double quat_y = std::stod(tokens[6]); const double quat_z = std::stod(tokens[7]); Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); - pose.block<3, 3>(0, 0) = Eigen::Quaternionf(quat_w, quat_x, quat_y, quat_z).toRotationMatrix(); - pose.block<3, 1>(0, 3) = Eigen::Vector3f(pose_x, pose_y, pose_z); + pose.block<3, 3>(0, 0) = Eigen::Quaternionf( + static_cast(quat_w), static_cast(quat_x), + static_cast(quat_y), static_cast(quat_z)) + .toRotationMatrix(); + pose.block<3, 1>(0, 3) = Eigen::Vector3f( + static_cast(pose_x), static_cast(pose_y), static_cast(pose_z)); pose_list.push_back(pose); } return pose_list; diff --git a/include/estimate_covariance/estimate_covariance.hpp b/include/estimate_covariance/estimate_covariance.hpp index 1b64ed9..35da2a7 100644 --- a/include/estimate_covariance/estimate_covariance.hpp +++ b/include/estimate_covariance/estimate_covariance.hpp @@ -5,6 +5,10 @@ #include +#include +#include +#include + namespace pclomp { @@ -17,17 +21,17 @@ struct ResultOfMultiNdtCovarianceEstimation }; /** \brief Estimate functions */ -Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation( +Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( const Eigen::Matrix & hessian); ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, - std::shared_ptr> - ndt_ptr, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, const std::vector & poses_to_search); ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( const NdtResult & ndt_result, - std::shared_ptr> - ndt_ptr, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, const std::vector & poses_to_search, const double temperature); /** \brief Find rotation matrix aligning covariance to principal axes diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h index b9d3407..2121c97 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h +++ b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h @@ -277,10 +277,10 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid } MultiVoxelGridCovariance(const MultiVoxelGridCovariance & other); - MultiVoxelGridCovariance(MultiVoxelGridCovariance && other); + MultiVoxelGridCovariance(MultiVoxelGridCovariance && other) noexcept; MultiVoxelGridCovariance & operator=(const MultiVoxelGridCovariance & other); - MultiVoxelGridCovariance & operator=(MultiVoxelGridCovariance && other); + MultiVoxelGridCovariance & operator=(MultiVoxelGridCovariance && other) noexcept; /** \brief Add a cloud to the voxel grid list and build a ND voxel grid from it. */ @@ -329,10 +329,6 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { sync(); - if (thread_num <= 0) { - thread_num_ = 1; - } - thread_num_ = thread_num; thread_futs_.resize(thread_num_); processing_inputs_.resize(thread_num_); diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp index 2568a70..d6638c3 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp +++ b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp @@ -57,6 +57,12 @@ #include #include +#include +#include +#include +#include +#include + namespace pclomp { @@ -83,7 +89,8 @@ MultiVoxelGridCovariance::MultiVoxelGridCovariance(const MultiVoxelGridC } template -MultiVoxelGridCovariance::MultiVoxelGridCovariance(MultiVoxelGridCovariance && other) +MultiVoxelGridCovariance::MultiVoxelGridCovariance( + MultiVoxelGridCovariance && other) noexcept : pcl::VoxelGrid(std::move(other)), voxel_centroids_ptr_(std::move(other.voxel_centroids_ptr_)), sid_to_iid_(std::move(other.sid_to_iid_)), @@ -126,9 +133,8 @@ MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::ope template MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::operator=( - MultiVoxelGridCovariance && other) + MultiVoxelGridCovariance && other) noexcept { - pcl::VoxelGrid::operator=(std::move(other)); voxel_centroids_ptr_ = std::move(other.voxel_centroids_ptr_); sid_to_iid_ = std::move(other.sid_to_iid_); grid_list_ = std::move(other.grid_list_); @@ -141,6 +147,8 @@ MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::ope setThreadNum(other.thread_num_); last_check_tid_ = -1; + pcl::VoxelGrid::operator=(std::move(other)); + return *this; } @@ -301,7 +309,8 @@ void MultiVoxelGridCovariance::applyFilter( return; } - Eigen::Vector4f min_p, max_p; + Eigen::Vector4f min_p; + Eigen::Vector4f max_p; pcl::getMinMax3D(*input, min_p, max_p); // Check that the leaf size is not too small, given the size of the data diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h index b126a09..48a89f8 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp.h +++ b/include/multigrid_pclomp/multigrid_ndt_omp.h @@ -116,12 +116,13 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration getHessian() const { return hessian_; } /** \brief Return the transformation array */ - inline const std::vector> + inline const std::vector> & getFinalTransformationArray() const { return transformation_array_; @@ -548,7 +549,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration> transformation_array_; std::vector transform_probability_array_; std::vector nearest_voxel_transformation_likelihood_array_; - double nearest_voxel_transformation_likelihood_; + double nearest_voxel_transformation_likelihood_{}; boost::optional regularization_pose_; Eigen::Vector3f regularization_pose_translation_; diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index b27443e..5e6b9a2 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -57,6 +57,10 @@ #include "multigrid_pclomp/multigrid_ndt_omp.h" +#include +#include +#include + namespace pclomp { @@ -87,7 +91,7 @@ MultiGridNormalDistributionsTransform:: template MultiGridNormalDistributionsTransform:: - MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform && other) + MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform && other) noexcept : BaseRegType(std::move(other)), target_cells_(std::move(other.target_cells_)), params_(std::move(other.params_)) @@ -114,8 +118,6 @@ MultiGridNormalDistributionsTransform & MultiGridNormalDistributionsTransform::operator=( const MultiGridNormalDistributionsTransform & other) { - BaseRegType::operator=(other); - target_cells_ = other.target_cells_; params_ = other.params_; @@ -135,16 +137,16 @@ MultiGridNormalDistributionsTransform::operator=( regularization_pose_ = other.regularization_pose_; regularization_pose_translation_ = other.regularization_pose_translation_; + BaseRegType::operator=(other); + return *this; } template MultiGridNormalDistributionsTransform & MultiGridNormalDistributionsTransform::operator=( - MultiGridNormalDistributionsTransform && other) + MultiGridNormalDistributionsTransform && other) noexcept { - BaseRegType::operator=(std::move(other)); - target_cells_ = std::move(other.target_cells_); params_ = std::move(other.params_); @@ -164,6 +166,8 @@ MultiGridNormalDistributionsTransform::operator=( regularization_pose_ = other.regularization_pose_; regularization_pose_translation_ = other.regularization_pose_translation_; + BaseRegType::operator=(std::move(other)); + return *this; } @@ -190,7 +194,8 @@ MultiGridNormalDistributionsTransform< params_.regularization_scale_factor = 0.0f; params_.use_line_search = false; - double gauss_c1, gauss_c2; + double gauss_c1; + double gauss_c2; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10.0 * (1 - outlier_ratio_); @@ -208,7 +213,8 @@ void MultiGridNormalDistributionsTransform::computeTra nr_iterations_ = 0; converged_ = false; - double gauss_c1, gauss_c2; + double gauss_c1; + double gauss_c2; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10 * (1 - outlier_ratio_); @@ -234,7 +240,9 @@ void MultiGridNormalDistributionsTransform::computeTra nearest_voxel_transformation_likelihood_array_.clear(); // Convert initial guess matrix to 6 element transformation vector - Eigen::Matrix p, delta_p, score_gradient; + Eigen::Matrix p; + Eigen::Matrix delta_p; + Eigen::Matrix score_gradient; Eigen::Vector3f init_translation = eig_transformation.translation(); Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2); @@ -498,7 +506,12 @@ void MultiGridNormalDistributionsTransform::computeAng Eigen::Matrix & p, bool compute_hessian) { // Simplified math for near 0 angles - double cx, cy, cz, sx, sy, sz; + double cx; + double cy; + double cz; + double sx; + double sy; + double sz; if (fabs(p(3)) < 10e-5) { // p(3) = 0; cx = 1.0; @@ -786,14 +799,14 @@ bool MultiGridNormalDistributionsTransform::updateInte return (false); } // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] - else if (g_t * (a_l - a_t) > 0) { + if (g_t * (a_l - a_t) > 0) { a_l = a_t; f_l = f_t; g_l = g_t; return (false); } // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] - else if (g_t * (a_l - a_t) < 0) { + if (g_t * (a_l - a_t) < 0) { a_u = a_l; f_u = f_l; g_u = g_l; @@ -804,8 +817,7 @@ bool MultiGridNormalDistributionsTransform::updateInte return (false); } // Interval Converged - else - return (true); + return (true); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -827,13 +839,13 @@ double MultiGridNormalDistributionsTransform::trialVal // Equation 2.4.2 [Sun, Yuan 2006] double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); - if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) + if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) { return (a_c); - else - return (0.5 * (a_q + a_c)); + } + return (0.5 * (a_q + a_c)); } // Case 2 in Trial Value Selection [More, Thuente 1994] - else if (g_t * g_l < 0) { + if (g_t * g_l < 0) { // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; @@ -845,13 +857,13 @@ double MultiGridNormalDistributionsTransform::trialVal // Equation 2.4.5 [Sun, Yuan 2006] double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; - if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) + if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) { return (a_c); - else - return (a_s); + } + return (a_s); } // Case 3 in Trial Value Selection [More, Thuente 1994] - else if (std::fabs(g_t) <= std::fabs(g_l)) { + if (std::fabs(g_t) <= std::fabs(g_l)) { // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; @@ -869,20 +881,18 @@ double MultiGridNormalDistributionsTransform::trialVal else a_t_next = a_s; - if (a_t > a_l) + if (a_t > a_l) { return (std::min(a_t + 0.66 * (a_u - a_t), a_t_next)); - else - return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); + } + return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); } // Case 4 in Trial Value Selection [More, Thuente 1994] - else { - // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t - // Equation 2.4.52 [Sun, Yuan 2006] - double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; - double w = std::sqrt(z * z - g_t * g_u); - // Equation 2.4.56 [Sun, Yuan 2006] - return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); - } + // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; + double w = std::sqrt(z * z - g_t * g_u); + // Equation 2.4.56 [Sun, Yuan 2006] + return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -901,13 +911,11 @@ double MultiGridNormalDistributionsTransform::computeS if (d_phi_0 >= 0) { // Not a decent direction - if (d_phi_0 == 0) - return 0; - else { - // Reverse step direction and calculate optimal step. - d_phi_0 *= -1; - step_dir *= -1; - } + if (d_phi_0 == 0) return 0; + + // Reverse step direction and calculate optimal step. + d_phi_0 *= -1; + step_dir *= -1; } // The Search Algorithm for T(mu) [More, Thuente 1994] @@ -921,7 +929,8 @@ double MultiGridNormalDistributionsTransform::computeS double nu = 0.9; // Initial endpoints of Interval I, - double a_l = 0, a_u = 0; + double a_l = 0; + double a_u = 0; // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 // [More, Thuente 1994] @@ -933,7 +942,8 @@ double MultiGridNormalDistributionsTransform::computeS // Check used to allow More-Thuente step length calculation to be skipped by making step_min == // step_max - bool interval_converged = (step_max - step_min) < 0, open_interval = true; + bool interval_converged = (step_max - step_min) < 0; + bool open_interval = true; double a_t = step_init; a_t = std::min(a_t, step_max); diff --git a/include/pclomp/gicp_omp.h b/include/pclomp/gicp_omp.h index 29840f7..c71d77e 100644 --- a/include/pclomp/gicp_omp.h +++ b/include/pclomp/gicp_omp.h @@ -340,7 +340,7 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint { - OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint * gicp) + explicit OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint * gicp) : BFGSDummyFunctor(), gicp_(gicp) { } diff --git a/include/pclomp/gicp_omp_impl.hpp b/include/pclomp/gicp_omp_impl.hpp index b34bad8..2ff8b5e 100644 --- a/include/pclomp/gicp_omp_impl.hpp +++ b/include/pclomp/gicp_omp_impl.hpp @@ -44,6 +44,9 @@ #include #include +#include +#include +#include //////////////////////////////////////////////////////////////////////////////////////// template @@ -52,7 +55,7 @@ void pclomp::GeneralizedIterativeClosestPoint::compute typename pcl::PointCloud::ConstPtr cloud, const typename pcl::search::KdTree::ConstPtr kdtree, MatricesVector & cloud_covariances) { - if (k_correspondences_ > int(cloud->size())) { + if (k_correspondences_ > static_cast(cloud->size())) { PCL_ERROR( "[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number of points in cloud (%lu) " "is less than k_correspondences_ (%lu)!\n", @@ -240,12 +243,13 @@ void pclomp::GeneralizedIterativeClosestPoint:: PCL_DEBUG("BFGS solver finished with exit code %i \n", result); transformation_matrix.setIdentity(); applyState(transformation_matrix, x); - } else + } else { PCL_THROW_EXCEPTION( pcl::SolverDidntConvergeException, "[pcl::" << getClassName() << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't " "converge!"); + } } //////////////////////////////////////////////////////////////////////////////////////// @@ -370,7 +374,7 @@ inline void pclomp::GeneralizedIterativeClosestPoint:: .template cast() * res); // Increment total error - f += double(res.transpose() * temp); + f += static_cast(res.transpose() * temp); // Increment translation gradient // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes) g.head<3>() += temp; @@ -379,8 +383,8 @@ inline void pclomp::GeneralizedIterativeClosestPoint:: // Increment rotation gradient R += p_src3 * temp.transpose(); } - f /= double(m); - g.head<3>() *= double(2.0 / m); + f /= static_cast(m); + g.head<3>() *= static_cast(2.0 / m); R *= 2.0 / m; gicp_->computeRDerivative(x, R, g); } @@ -392,7 +396,6 @@ pclomp::GeneralizedIterativeClosestPoint::computeTrans PointCloudSource & output, const Eigen::Matrix4f & guess) { pcl::IterativeClosestPoint::initComputeReciprocal(); - using namespace std; // Difference between consecutive transforms double delta = 0; // Get the size of the target @@ -436,7 +439,8 @@ pclomp::GeneralizedIterativeClosestPoint::computeTrans for (size_t i = 0; i < 4; i++) for (size_t j = 0; j < 4; j++) for (size_t k = 0; k < 4; k++) - transform_R(i, j) += double(transformation_(i, k)) * double(guess(k, j)); + transform_R(i, j) += + static_cast(transformation_(i, k)) * static_cast(guess(k, j)); const Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>(); @@ -533,8 +537,9 @@ pclomp::GeneralizedIterativeClosestPoint::computeTrans "Transformation difference: %f\n", getClassName().c_str(), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array().abs().sum()); - } else + } else { PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n", getClassName().c_str()); + } } final_transformation_ = previous_transformation_ * guess; diff --git a/include/pclomp/ndt_omp.h b/include/pclomp/ndt_omp.h index 176b240..4130e28 100644 --- a/include/pclomp/ndt_omp.h +++ b/include/pclomp/ndt_omp.h @@ -182,7 +182,7 @@ class NormalDistributionsTransform : public pcl::Registration getHessian() const { return hessian_; } /** \brief Return the transformation array */ - inline const std::vector> + inline const std::vector> & getFinalTransformationArray() const { return transformation_array_; @@ -322,7 +322,7 @@ class NormalDistributionsTransform : public pcl::Registration & score_gradient, Eigen::Matrix & hessian, - const Eigen::Matrix & point_gradient_, + const Eigen::Matrix & point_gradient4, const Eigen::Matrix & point_hessian_, const Eigen::Vector3d & x_trans, const Eigen::Matrix3d & c_inv, bool compute_hessian = true) const; diff --git a/include/pclomp/ndt_omp_impl.hpp b/include/pclomp/ndt_omp_impl.hpp index ea48839..4bc4188 100644 --- a/include/pclomp/ndt_omp_impl.hpp +++ b/include/pclomp/ndt_omp_impl.hpp @@ -1,4 +1,6 @@ #include "ndt_omp.h" + +#include /* * Software License Agreement (BSD License) * @@ -42,6 +44,9 @@ #ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_ #define PCL_REGISTRATION_NDT_OMP_IMPL_H_ +#include +#include + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template pclomp::NormalDistributionsTransform::NormalDistributionsTransform() @@ -51,30 +56,7 @@ pclomp::NormalDistributionsTransform::NormalDistributi gauss_d2_(), gauss_d3_(), trans_probability_(), - regularization_pose_(boost::none), - j_ang_a_(), - j_ang_b_(), - j_ang_c_(), - j_ang_d_(), - j_ang_e_(), - j_ang_f_(), - j_ang_g_(), - j_ang_h_(), - h_ang_a2_(), - h_ang_a3_(), - h_ang_b2_(), - h_ang_b3_(), - h_ang_c2_(), - h_ang_c3_(), - h_ang_d1_(), - h_ang_d2_(), - h_ang_d3_(), - h_ang_e1_(), - h_ang_e2_(), - h_ang_e3_(), - h_ang_f1_(), - h_ang_f2_(), - h_ang_f3_() + regularization_pose_(boost::none) { reg_name_ = "NormalDistributionsTransform"; @@ -87,7 +69,8 @@ pclomp::NormalDistributionsTransform::NormalDistributi params_.regularization_scale_factor = 0.0f; params_.use_line_search = false; - double gauss_c1, gauss_c2; + double gauss_c1; + double gauss_c2; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10.0 * (1 - outlier_ratio_); @@ -105,7 +88,8 @@ void pclomp::NormalDistributionsTransform::computeTran nr_iterations_ = 0; converged_ = false; - double gauss_c1, gauss_c2; + double gauss_c1; + double gauss_c2; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10 * (1 - outlier_ratio_); @@ -127,7 +111,9 @@ void pclomp::NormalDistributionsTransform::computeTran transformation_array_.push_back(final_transformation_); // Convert initial guess matrix to 6 element transformation vector - Eigen::Matrix p, delta_p, score_gradient; + Eigen::Matrix p; + Eigen::Matrix delta_p; + Eigen::Matrix score_gradient; Eigen::Vector3f init_translation = eig_transformation.translation(); Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2); p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), @@ -268,20 +254,22 @@ double pclomp::NormalDistributionsTransform::computeDe int thread_n = omp_get_thread_num(); // Original Point and Transformed Point - PointSource x_pt, x_trans_pt; + PointSource x_pt; + PointSource x_trans_pt; // Original Point and Transformed Point (for math) - Eigen::Vector3d x, x_trans; + Eigen::Vector3d x; + Eigen::Vector3d x_trans; // Occupied Voxel TargetGridLeafConstPtr cell; // Inverse Covariance of Occupied Voxel Eigen::Matrix3d c_inv; // Initialize Point Gradient and Hessian - Eigen::Matrix point_gradient_; - Eigen::Matrix point_hessian_; - point_gradient_.setZero(); - point_gradient_.block<3, 3>(0, 0).setIdentity(); - point_hessian_.setZero(); + Eigen::Matrix point_gradient; + Eigen::Matrix point_hessian; + point_gradient.setZero(); + point_gradient.block<3, 3>(0, 0).setIdentity(); + point_hessian.setZero(); x_trans_pt = trans_cloud.points[idx]; @@ -313,7 +301,7 @@ double pclomp::NormalDistributionsTransform::computeDe for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); - neighborhood_it != neighborhood.end(); neighborhood_it++) { + neighborhood_it != neighborhood.end(); ++neighborhood_it) { cell = *neighborhood_it; x_pt = input_->points[idx]; x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z); @@ -327,11 +315,11 @@ double pclomp::NormalDistributionsTransform::computeDe // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in // Equations 6.18 and 6.20 [Magnusson 2009] - computePointDerivatives(x, point_gradient_, point_hessian_); + computePointDerivatives(x, point_gradient, point_hessian); // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to // Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] double score_pt = updateDerivatives( - score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, + score_gradient_pt, hessian_pt, point_gradient, point_hessian, x_trans, c_inv, compute_hessian); neighborhood_count++; sum_score_pt += score_pt; @@ -412,7 +400,12 @@ void pclomp::NormalDistributionsTransform::computeAngl Eigen::Matrix & p, bool compute_hessian) { // Simplified math for near 0 angles - double cx, cy, cz, sx, sy, sz; + double cx; + double cy; + double cz; + double sx; + double sy; + double sz; if (fabs(p(3)) < 10e-5) { // p(3) = 0; cx = 1.0; @@ -450,18 +443,28 @@ void pclomp::NormalDistributionsTransform::computeAngl j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0; j_ang.setZero(); - j_ang.row(0).noalias() = - Eigen::Vector4f((-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f); - j_ang.row(1).noalias() = - Eigen::Vector4f((cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f); - j_ang.row(2).noalias() = Eigen::Vector4f((-sy * cz), sy * sz, cy, 0.0f); - j_ang.row(3).noalias() = Eigen::Vector4f(sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f); - j_ang.row(4).noalias() = Eigen::Vector4f((-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f); - j_ang.row(5).noalias() = Eigen::Vector4f((-cy * sz), (-cy * cz), 0, 0.0f); - j_ang.row(6).noalias() = - Eigen::Vector4f((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 0.0f); - j_ang.row(7).noalias() = - Eigen::Vector4f((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 0.0f); + j_ang.row(0).noalias() = Eigen::Vector4f( + static_cast(-sx * sz + cx * sy * cz), static_cast(-sx * cz - cx * sy * sz), + static_cast(-cx * cy), 0.0f); + j_ang.row(1).noalias() = Eigen::Vector4f( + static_cast(cx * sz + sx * sy * cz), static_cast(cx * cz - sx * sy * sz), + static_cast(-sx * cy), 0.0f); + j_ang.row(2).noalias() = Eigen::Vector4f( + static_cast(-sy * cz), static_cast(sy * sz), static_cast(cy), 0.0f); + j_ang.row(3).noalias() = Eigen::Vector4f( + static_cast(sx * cy * cz), static_cast(-sx * cy * sz), + static_cast(sx * sy), 0.0f); + j_ang.row(4).noalias() = Eigen::Vector4f( + static_cast(-cx * cy * cz), static_cast(cx * cy * sz), + static_cast(-cx * sy), 0.0f); + j_ang.row(5).noalias() = + Eigen::Vector4f(static_cast(-cy * sz), static_cast(-cy * cz), 0.0f, 0.0f); + j_ang.row(6).noalias() = Eigen::Vector4f( + static_cast(cx * cz - sx * sy * sz), static_cast(-cx * sz - sx * sy * cz), 0, + 0.0f); + j_ang.row(7).noalias() = Eigen::Vector4f( + static_cast(sx * cz + cx * sy * sz), static_cast(cx * sy * cz - sx * sz), 0, + 0.0f); if (compute_hessian) { // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers @@ -488,36 +491,52 @@ void pclomp::NormalDistributionsTransform::computeAngl h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0; h_ang.setZero(); - h_ang.row(0).noalias() = - Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f); // a2 + h_ang.row(0).noalias() = Eigen::Vector4f( + static_cast(-cx * sz - sx * sy * cz), static_cast(-cx * cz + sx * sy * sz), + static_cast(sx * cy), 0.0f); // a2 h_ang.row(1).noalias() = Eigen::Vector4f( - (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f); // a3 - - h_ang.row(2).noalias() = - Eigen::Vector4f((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f); // b2 - h_ang.row(3).noalias() = - Eigen::Vector4f((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f); // b3 - - h_ang.row(4).noalias() = - Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f); // c2 - h_ang.row(5).noalias() = - Eigen::Vector4f((cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f); // c3 - - h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f); // d1 - h_ang.row(7).noalias() = - Eigen::Vector4f((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f); // d2 - h_ang.row(8).noalias() = - Eigen::Vector4f((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f); // d3 - - h_ang.row(9).noalias() = Eigen::Vector4f((sy * sz), (sy * cz), 0, 0.0f); // e1 - h_ang.row(10).noalias() = Eigen::Vector4f((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f); // e2 - h_ang.row(11).noalias() = Eigen::Vector4f((cx * cy * sz), (cx * cy * cz), 0, 0.0f); // e3 - - h_ang.row(12).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), 0, 0.0f); // f1 - h_ang.row(13).noalias() = - Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f); // f2 - h_ang.row(14).noalias() = - Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f); // f3 + static_cast(-sx * sz + cx * sy * cz), static_cast(-cx * sy * sz - sx * cz), + static_cast(-cx * cy), 0.0f); // a3 + + h_ang.row(2).noalias() = Eigen::Vector4f( + static_cast(cx * cy * cz), static_cast(-cx * cy * sz), + static_cast(cx * sy), 0.0f); // b2 + h_ang.row(3).noalias() = Eigen::Vector4f( + static_cast(sx * cy * cz), static_cast(-sx * cy * sz), + static_cast(sx * sy), 0.0f); // b3 + + h_ang.row(4).noalias() = Eigen::Vector4f( + static_cast(-sx * cz - cx * sy * sz), static_cast(sx * sz - cx * sy * cz), 0.0f, + 0.0f); // c2 + h_ang.row(5).noalias() = Eigen::Vector4f( + static_cast(cx * cz - sx * sy * sz), static_cast(-sx * sy * cz - cx * sz), 0.0f, + 0.0f); // c3 + + h_ang.row(6).noalias() = Eigen::Vector4f( + static_cast(-cy * cz), static_cast(cy * sz), static_cast(sy), + 0.0f); // d1 + h_ang.row(7).noalias() = Eigen::Vector4f( + static_cast(-sx * sy * cz), static_cast(sx * sy * sz), + static_cast(sx * cy), 0.0f); // d2 + h_ang.row(8).noalias() = Eigen::Vector4f( + static_cast(cx * sy * cz), static_cast(-cx * sy * sz), + static_cast(-cx * cy), 0.0f); // d3 + + h_ang.row(9).noalias() = + Eigen::Vector4f(static_cast(sy * sz), static_cast(sy * cz), 0.0f, 0.0f); // e1 + h_ang.row(10).noalias() = Eigen::Vector4f( + static_cast(-sx * cy * sz), static_cast(-sx * cy * cz), 0.0f, 0.0f); // e2 + h_ang.row(11).noalias() = Eigen::Vector4f( + static_cast(cx * cy * sz), static_cast(cx * cy * cz), 0.0f, 0.0f); // e3 + + h_ang.row(12).noalias() = + Eigen::Vector4f(static_cast(-cy * cz), static_cast(cy * sz), 0.0f, 0.0f); // f1 + h_ang.row(13).noalias() = Eigen::Vector4f( + static_cast(-cx * sz - sx * sy * cz), static_cast(-cx * cz + sx * sy * sz), + 0.0f, 0.0f); // f2 + h_ang.row(14).noalias() = Eigen::Vector4f( + static_cast(-sx * sz + cx * sy * cz), static_cast(-cx * sy * sz - sx * cz), + 0.0f, 0.0f); // f3 } } @@ -527,7 +546,8 @@ void pclomp::NormalDistributionsTransform::computePoin Eigen::Vector3d & x, Eigen::Matrix & point_gradient_, Eigen::Matrix & point_hessian_, bool compute_hessian) const { - Eigen::Vector4f x4(x[0], x[1], x[2], 0.0f); + Eigen::Vector4f x4( + static_cast(x[0]), static_cast(x[1]), static_cast(x[2]), 0.0f); // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 @@ -589,7 +609,12 @@ void pclomp::NormalDistributionsTransform::computePoin if (compute_hessian) { // Vectors from Equation 6.21 [Magnusson 2009] - Eigen::Vector3d a, b, c, d, e, f; + Eigen::Vector3d a; + Eigen::Vector3d b; + Eigen::Vector3d c; + Eigen::Vector3d d; + Eigen::Vector3d e; + Eigen::Vector3d f; a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_); b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_); @@ -621,7 +646,9 @@ double pclomp::NormalDistributionsTransform::updateDer const Eigen::Matrix & point_hessian_, const Eigen::Vector3d & x_trans, const Eigen::Matrix3d & c_inv, bool compute_hessian) const { - Eigen::Matrix x_trans4(x_trans[0], x_trans[1], x_trans[2], 0.0f); + Eigen::Matrix x_trans4( + static_cast(x_trans[0]), static_cast(x_trans[1]), static_cast(x_trans[2]), + 0.0f); Eigen::Matrix4f c_inv4 = Eigen::Matrix4f::Zero(); c_inv4.topLeftCorner(3, 3) = c_inv.cast(); @@ -679,20 +706,22 @@ void pclomp::NormalDistributionsTransform::computeHess Eigen::Matrix &) { // Original Point and Transformed Point - PointSource x_pt, x_trans_pt; + PointSource x_pt; + PointSource x_trans_pt; // Original Point and Transformed Point (for math) - Eigen::Vector3d x, x_trans; + Eigen::Vector3d x; + Eigen::Vector3d x_trans; // Occupied Voxel TargetGridLeafConstPtr cell; // Inverse Covariance of Occupied Voxel Eigen::Matrix3d c_inv; // Initialize Point Gradient and Hessian - Eigen::Matrix point_gradient_; - Eigen::Matrix point_hessian_; - point_gradient_.setZero(); - point_gradient_.block<3, 3>(0, 0).setIdentity(); - point_hessian_.setZero(); + Eigen::Matrix point_gradient; + Eigen::Matrix point_hessian; + point_gradient.setZero(); + point_gradient.block<3, 3>(0, 0).setIdentity(); + point_hessian.setZero(); hessian.setZero(); @@ -724,7 +753,7 @@ void pclomp::NormalDistributionsTransform::computeHess for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); - neighborhood_it != neighborhood.end(); neighborhood_it++) { + neighborhood_it != neighborhood.end(); ++neighborhood_it) { cell = *neighborhood_it; { @@ -740,10 +769,10 @@ void pclomp::NormalDistributionsTransform::computeHess // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in // Equations 6.18 and 6.20 [Magnusson 2009] - computePointDerivatives(x, point_gradient_, point_hessian_); + computePointDerivatives(x, point_gradient, point_hessian); // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, // respectively [Magnusson 2009] - updateHessian(hessian, point_gradient_, point_hessian_, x_trans, c_inv); + updateHessian(hessian, point_gradient, point_hessian, x_trans, c_inv); } } } @@ -794,14 +823,14 @@ bool pclomp::NormalDistributionsTransform::updateInter return (false); } // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] - else if (g_t * (a_l - a_t) > 0) { + if (g_t * (a_l - a_t) > 0) { a_l = a_t; f_l = f_t; g_l = g_t; return (false); } // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] - else if (g_t * (a_l - a_t) < 0) { + if (g_t * (a_l - a_t) < 0) { a_u = a_l; f_u = f_l; g_u = g_l; @@ -812,8 +841,7 @@ bool pclomp::NormalDistributionsTransform::updateInter return (false); } // Interval Converged - else - return (true); + return (true); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -835,13 +863,13 @@ double pclomp::NormalDistributionsTransform::trialValu // Equation 2.4.2 [Sun, Yuan 2006] double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); - if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) + if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) { return (a_c); - else - return (0.5 * (a_q + a_c)); + } + return (0.5 * (a_q + a_c)); } // Case 2 in Trial Value Selection [More, Thuente 1994] - else if (g_t * g_l < 0) { + if (g_t * g_l < 0) { // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; @@ -853,13 +881,13 @@ double pclomp::NormalDistributionsTransform::trialValu // Equation 2.4.5 [Sun, Yuan 2006] double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; - if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) + if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) { return (a_c); - else - return (a_s); + } + return (a_s); } // Case 3 in Trial Value Selection [More, Thuente 1994] - else if (std::fabs(g_t) <= std::fabs(g_l)) { + if (std::fabs(g_t) <= std::fabs(g_l)) { // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; @@ -877,20 +905,18 @@ double pclomp::NormalDistributionsTransform::trialValu else a_t_next = a_s; - if (a_t > a_l) + if (a_t > a_l) { return (std::min(a_t + 0.66 * (a_u - a_t), a_t_next)); - else - return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); + } + return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); } // Case 4 in Trial Value Selection [More, Thuente 1994] - else { - // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t - // Equation 2.4.52 [Sun, Yuan 2006] - double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; - double w = std::sqrt(z * z - g_t * g_u); - // Equation 2.4.56 [Sun, Yuan 2006] - return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); - } + // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; + double w = std::sqrt(z * z - g_t * g_u); + // Equation 2.4.56 [Sun, Yuan 2006] + return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -909,13 +935,11 @@ double pclomp::NormalDistributionsTransform::computeSt if (d_phi_0 >= 0) { // Not a decent direction - if (d_phi_0 == 0) - return 0; - else { - // Reverse step direction and calculate optimal step. - d_phi_0 *= -1; - step_dir *= -1; - } + if (d_phi_0 == 0) return 0; + + // Reverse step direction and calculate optimal step. + d_phi_0 *= -1; + step_dir *= -1; } // The Search Algorithm for T(mu) [More, Thuente 1994] @@ -929,7 +953,8 @@ double pclomp::NormalDistributionsTransform::computeSt double nu = 0.9; // Initial endpoints of Interval I, - double a_l = 0, a_u = 0; + double a_l = 0; + double a_u = 0; // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 // [More, Thuente 1994] @@ -941,7 +966,8 @@ double pclomp::NormalDistributionsTransform::computeSt // Check used to allow More-Thuente step length calculation to be skipped by making step_min == // step_max - bool interval_converged = (step_max - step_min) < 0, open_interval = true; + bool interval_converged = (step_max - step_min) < 0; + bool open_interval = true; double a_t = step_init; a_t = std::min(a_t, step_max); @@ -1091,7 +1117,7 @@ double pclomp::NormalDistributionsTransform::calculate for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); - neighborhood_it != neighborhood.end(); neighborhood_it++) { + neighborhood_it != neighborhood.end(); ++neighborhood_it) { TargetGridLeafConstPtr cell = *neighborhood_it; Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); @@ -1148,7 +1174,7 @@ pclomp::NormalDistributionsTransform::calculateTransfo for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); - neighborhood_it != neighborhood.end(); neighborhood_it++) { + neighborhood_it != neighborhood.end(); ++neighborhood_it) { TargetGridLeafConstPtr cell = *neighborhood_it; Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); @@ -1206,7 +1232,7 @@ double pclomp::NormalDistributionsTransform:: for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); - neighborhood_it != neighborhood.end(); neighborhood_it++) { + neighborhood_it != neighborhood.end(); ++neighborhood_it) { TargetGridLeafConstPtr cell = *neighborhood_it; Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); diff --git a/include/pclomp/ndt_struct.hpp b/include/pclomp/ndt_struct.hpp index b8cdcf9..b613a8e 100644 --- a/include/pclomp/ndt_struct.hpp +++ b/include/pclomp/ndt_struct.hpp @@ -89,13 +89,13 @@ struct NdtResult struct NdtParams { - double trans_epsilon; - double step_size; - double resolution; - int max_iterations; - NeighborSearchMethod search_method; - int num_threads; - float regularization_scale_factor; + double trans_epsilon{}; + double step_size{}; + double resolution{}; + int max_iterations{}; + NeighborSearchMethod search_method{}; + int num_threads{}; + float regularization_scale_factor{}; // line search is false by default // "use_lines_search = true" is not tested well diff --git a/include/pclomp/voxel_grid_covariance_omp.h b/include/pclomp/voxel_grid_covariance_omp.h index db8b0dc..fce145f 100644 --- a/include/pclomp/voxel_grid_covariance_omp.h +++ b/include/pclomp/voxel_grid_covariance_omp.h @@ -386,7 +386,7 @@ class VoxelGridCovariance : public pcl::VoxelGrid // Find leaves corresponding to neighbors k_leaves.reserve(k); - for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); iter++) { + for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); ++iter) { k_leaves.push_back(&leaves_[voxel_centroids_leaf_indices_[*iter]]); } return k; @@ -436,7 +436,7 @@ class VoxelGridCovariance : public pcl::VoxelGrid // Find leaves corresponding to neighbors k_leaves.reserve(k); - for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); iter++) { + for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); ++iter) { auto leaf = leaves_.find(voxel_centroids_leaf_indices_[*iter]); if (leaf == leaves_.end()) { std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl; diff --git a/include/pclomp/voxel_grid_covariance_omp_impl.hpp b/include/pclomp/voxel_grid_covariance_omp_impl.hpp index 0f15bee..81d8b07 100644 --- a/include/pclomp/voxel_grid_covariance_omp_impl.hpp +++ b/include/pclomp/voxel_grid_covariance_omp_impl.hpp @@ -46,6 +46,10 @@ #include #include +#include +#include +#include + ////////////////////////////////////////////////////////////////////////////////////////// template void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) @@ -65,7 +69,8 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) output.is_dense = true; // we filter out invalid points output.points.clear(); - Eigen::Vector4f min_p, max_p; + Eigen::Vector4f min_p; + Eigen::Vector4f max_p; // Get the minimum and maximum dimensions if (!filter_field_name_.empty()) // If we don't want to process the entire cloud... pcl::getMinMax3D( @@ -117,7 +122,7 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) rgba_index = pcl::getFieldIndex("rgb", fields); if (rgba_index == -1) rgba_index = pcl::getFieldIndex("rgba", fields); if (rgba_index >= 0) { - rgba_index = fields[rgba_index].offset; + rgba_index = static_cast(fields[rgba_index].offset); centroid_size += 3; } @@ -142,7 +147,7 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) continue; // Get the distance value - const uint8_t * pt_data = reinterpret_cast(&input_->points[cp]); + const auto * pt_data = reinterpret_cast(&input_->points[cp]); float distance_value = 0; memcpy(&distance_value, pt_data + fields[distance_idx].offset, sizeof(float)); @@ -186,12 +191,13 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // ---[ RGB special case if (rgba_index >= 0) { // fill r/g/b data - int rgb; + unsigned int rgb; memcpy( - &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(int)); - centroid[centroid_size - 3] = static_cast((rgb >> 16) & 0x0000ff); - centroid[centroid_size - 2] = static_cast((rgb >> 8) & 0x0000ff); - centroid[centroid_size - 1] = static_cast((rgb) & 0x0000ff); + &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, + sizeof(unsigned int)); + centroid[centroid_size - 3] = static_cast((rgb >> 16u) & 0x0000ffu); + centroid[centroid_size - 2] = static_cast((rgb >> 8u) & 0x0000ffu); + centroid[centroid_size - 1] = static_cast((rgb) & 0x0000ffu); } pcl::for_each_type( pcl::NdCopyPointEigenFunctor(input_->points[cp], centroid)); @@ -199,9 +205,8 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) } ++leaf.nr_points; } - } - // No distance filtering, process all data - else { + // No distance filtering, process all data + } else { // First pass: go over all points and insert them into the right leaf for (size_t cp = 0; cp < input_->points.size(); ++cp) { if (!input_->is_dense) @@ -245,12 +250,12 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // ---[ RGB special case if (rgba_index >= 0) { // Fill r/g/b data, assuming that the order is BGRA - int rgb; + unsigned int rgb; memcpy( &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(int)); - centroid[centroid_size - 3] = static_cast((rgb >> 16) & 0x0000ff); - centroid[centroid_size - 2] = static_cast((rgb >> 8) & 0x0000ff); - centroid[centroid_size - 1] = static_cast((rgb) & 0x0000ff); + centroid[centroid_size - 3] = static_cast((rgb >> 16u) & 0x0000ffu); + centroid[centroid_size - 2] = static_cast((rgb >> 8u) & 0x0000ffu); + centroid[centroid_size - 1] = static_cast((rgb) & 0x0000ffu); } pcl::for_each_type( pcl::NdCopyPointEigenFunctor(input_->points[cp], centroid)); @@ -305,10 +310,12 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // ---[ RGB special case if (rgba_index >= 0) { // pack r/g/b into rgb - float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], - b = leaf.centroid[centroid_size - 1]; - int rgb = - (static_cast(r)) << 16 | (static_cast(g)) << 8 | (static_cast(b)); + float r = leaf.centroid[centroid_size - 3]; + float g = leaf.centroid[centroid_size - 2]; + float b = leaf.centroid[centroid_size - 1]; + int rgb = static_cast( + (static_cast(r)) << 16u | (static_cast(g)) << 8u | + (static_cast(b))); memcpy(reinterpret_cast(&output.points.back()) + rgba_index, &rgb, sizeof(float)); } } diff --git a/src/estimate_covariance/estimate_covariance.cpp b/src/estimate_covariance/estimate_covariance.cpp index b01a02e..120d66f 100644 --- a/src/estimate_covariance/estimate_covariance.cpp +++ b/src/estimate_covariance/estimate_covariance.cpp @@ -6,18 +6,18 @@ namespace pclomp { -Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation( +Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( const Eigen::Matrix & hessian) { const Eigen::Matrix2d hessian_xy = hessian.block<2, 2>(0, 0); - const Eigen::Matrix2d covariance_xy = -hessian_xy.inverse(); + Eigen::Matrix2d covariance_xy = -hessian_xy.inverse(); return covariance_xy; } ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, - std::shared_ptr> - ndt_ptr, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, const std::vector & poses_to_search) { // initialize by the main result @@ -52,8 +52,8 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( const NdtResult & ndt_result, - std::shared_ptr> - ndt_ptr, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, const std::vector & poses_to_search, const double temperature) { // initialize by the main result @@ -77,7 +77,7 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( NdtResult sub_ndt_result{}; sub_ndt_result.pose = curr_pose; sub_ndt_result.iteration_num = 0; - sub_ndt_result.nearest_voxel_transformation_likelihood = nvtl; + sub_ndt_result.nearest_voxel_transformation_likelihood = static_cast(nvtl); ndt_results.push_back(sub_ndt_result); } @@ -182,7 +182,7 @@ Eigen::Matrix2d adjust_diagonal_covariance( Eigen::Matrix2d cov_base_link = rotate_covariance_to_base_link(covariance, pose); cov_base_link(0, 0) = std::max(cov_base_link(0, 0), fixed_cov00); cov_base_link(1, 1) = std::max(cov_base_link(1, 1), fixed_cov11); - const Eigen::Matrix2d cov_map = rotate_covariance_to_map(cov_base_link, pose); + Eigen::Matrix2d cov_map = rotate_covariance_to_map(cov_base_link, pose); return cov_map; } From 379d9404b44100a45326f204b8a04d87f63cf937 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Mon, 22 Jul 2024 15:09:36 +0900 Subject: [PATCH 2/9] fix initializing Signed-off-by: a-maumau --- .../multigrid_ndt_omp_impl.hpp | 24 +++++++++---------- include/pclomp/ndt_omp_impl.hpp | 24 +++++++++---------- .../pclomp/voxel_grid_covariance_omp_impl.hpp | 6 ++--- 3 files changed, 27 insertions(+), 27 deletions(-) diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index 5e6b9a2..d337d3f 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -194,8 +194,8 @@ MultiGridNormalDistributionsTransform< params_.regularization_scale_factor = 0.0f; params_.use_line_search = false; - double gauss_c1; - double gauss_c2; + double gauss_c1 = NAN; + double gauss_c2 = NAN; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10.0 * (1 - outlier_ratio_); @@ -213,8 +213,8 @@ void MultiGridNormalDistributionsTransform::computeTra nr_iterations_ = 0; converged_ = false; - double gauss_c1; - double gauss_c2; + double gauss_c1 = NAN; + double gauss_c2 = NAN; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10 * (1 - outlier_ratio_); @@ -252,7 +252,7 @@ void MultiGridNormalDistributionsTransform::computeTra Eigen::Matrix hessian; double score = 0; - double delta_p_norm; + double delta_p_norm = NAN; if (regularization_pose_) { Eigen::Transform regularization_pose_transformation; @@ -506,12 +506,12 @@ void MultiGridNormalDistributionsTransform::computeAng Eigen::Matrix & p, bool compute_hessian) { // Simplified math for near 0 angles - double cx; - double cy; - double cz; - double sx; - double sy; - double sz; + double cx = NAN; + double cy = NAN; + double cz = NAN; + double sx = NAN; + double sy = NAN; + double sz = NAN; if (fabs(p(3)) < 10e-5) { // p(3) = 0; cx = 1.0; @@ -874,7 +874,7 @@ double MultiGridNormalDistributionsTransform::trialVal // Equation 2.4.5 [Sun, Yuan 2006] double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; - double a_t_next; + double a_t_next = NAN; if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) a_t_next = a_c; diff --git a/include/pclomp/ndt_omp_impl.hpp b/include/pclomp/ndt_omp_impl.hpp index 4bc4188..a49f0ad 100644 --- a/include/pclomp/ndt_omp_impl.hpp +++ b/include/pclomp/ndt_omp_impl.hpp @@ -69,8 +69,8 @@ pclomp::NormalDistributionsTransform::NormalDistributi params_.regularization_scale_factor = 0.0f; params_.use_line_search = false; - double gauss_c1; - double gauss_c2; + double gauss_c1 = NAN; + double gauss_c2 = NAN; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10.0 * (1 - outlier_ratio_); @@ -88,8 +88,8 @@ void pclomp::NormalDistributionsTransform::computeTran nr_iterations_ = 0; converged_ = false; - double gauss_c1; - double gauss_c2; + double gauss_c1 = NAN; + double gauss_c2 = NAN; // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10 * (1 - outlier_ratio_); @@ -122,7 +122,7 @@ void pclomp::NormalDistributionsTransform::computeTran Eigen::Matrix hessian; double score = 0; - double delta_p_norm; + double delta_p_norm = NAN; if (regularization_pose_) { Eigen::Transform regularization_pose_transformation; @@ -400,12 +400,12 @@ void pclomp::NormalDistributionsTransform::computeAngl Eigen::Matrix & p, bool compute_hessian) { // Simplified math for near 0 angles - double cx; - double cy; - double cz; - double sx; - double sy; - double sz; + double cx = NAN; + double cy = NAN; + double cz = NAN; + double sx = NAN; + double sy = NAN; + double sz = NAN; if (fabs(p(3)) < 10e-5) { // p(3) = 0; cx = 1.0; @@ -898,7 +898,7 @@ double pclomp::NormalDistributionsTransform::trialValu // Equation 2.4.5 [Sun, Yuan 2006] double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; - double a_t_next; + double a_t_next = NAN; if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) a_t_next = a_c; diff --git a/include/pclomp/voxel_grid_covariance_omp_impl.hpp b/include/pclomp/voxel_grid_covariance_omp_impl.hpp index 81d8b07..9243e2b 100644 --- a/include/pclomp/voxel_grid_covariance_omp_impl.hpp +++ b/include/pclomp/voxel_grid_covariance_omp_impl.hpp @@ -191,7 +191,7 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // ---[ RGB special case if (rgba_index >= 0) { // fill r/g/b data - unsigned int rgb; + unsigned int rgb = 0; memcpy( &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(unsigned int)); @@ -250,7 +250,7 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // ---[ RGB special case if (rgba_index >= 0) { // Fill r/g/b data, assuming that the order is BGRA - unsigned int rgb; + unsigned int rgb = 0; memcpy( &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(int)); centroid[centroid_size - 3] = static_cast((rgb >> 16u) & 0x0000ffu); @@ -278,7 +278,7 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max // eigen value. - double min_covar_eigvalue; + double min_covar_eigvalue = NAN; for (auto it = leaves_.begin(); it != leaves_.end(); ++it) { // Normalize the centroid From 76a9da9437722d68cf7a8e29abf011a2adb06189 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 6 Aug 2024 13:10:30 +0900 Subject: [PATCH 3/9] apply update in .cppcheck_suppressions Signed-off-by: a-maumau --- apps/check_covariance.cpp | 24 ++++++++++++------------ apps/pcd_map_grid_manager.hpp | 8 ++++---- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/apps/check_covariance.cpp b/apps/check_covariance.cpp index 67e51a1..aae78a0 100644 --- a/apps/check_covariance.cpp +++ b/apps/check_covariance.cpp @@ -181,14 +181,14 @@ int main(int argc, char ** argv) ofs_mndt << "index,score,initial_x,initial_y,result_x,result_y" << std::endl; ofs_mndt << std::fixed; for (int j = 0; j < n_mndt; j++) { - const pclomp::NdtResult & ndt_result = result_of_mndt.ndt_results[j]; - const auto nvtl = ndt_result.nearest_voxel_transformation_likelihood; + const pclomp::NdtResult & multi_ndt_result = result_of_mndt.ndt_results[j]; + const auto nvtl = multi_ndt_result.nearest_voxel_transformation_likelihood; const auto initial_x = result_of_mndt.ndt_initial_poses[j](0, 3); const auto initial_y = result_of_mndt.ndt_initial_poses[j](1, 3); - const auto result_x = ndt_result.pose(0, 3); - const auto result_y = ndt_result.pose(1, 3); - ofs_mndt << j << "," << nvtl << "," << initial_x << "," << initial_y << "," << result_x << "," - << result_y << std::endl; + const auto multi_ndt_result_x = multi_ndt_result.pose(0, 3); + const auto multi_ndt_result_y = multi_ndt_result.pose(1, 3); + ofs_mndt << j << "," << nvtl << "," << initial_x << "," << initial_y << "," + << multi_ndt_result_x << "," << multi_ndt_result_y << std::endl; } // output multi ndt score result @@ -197,14 +197,14 @@ int main(int argc, char ** argv) ofs_mndt_score << "index,score,initial_x,initial_y,result_x,result_y" << std::endl; ofs_mndt_score << std::fixed; for (int j = 0; j < n_mndt_score; j++) { - const pclomp::NdtResult & ndt_result = result_of_mndt_score.ndt_results[j]; - const auto nvtl = ndt_result.nearest_voxel_transformation_likelihood; + const pclomp::NdtResult & multi_ndt_score_result = result_of_mndt_score.ndt_results[j]; + const auto nvtl = multi_ndt_score_result.nearest_voxel_transformation_likelihood; const auto initial_x = result_of_mndt_score.ndt_initial_poses[j](0, 3); const auto initial_y = result_of_mndt_score.ndt_initial_poses[j](1, 3); - const auto result_x = ndt_result.pose(0, 3); - const auto result_y = ndt_result.pose(1, 3); - ofs_mndt_score << j << "," << nvtl << "," << initial_x << "," << initial_y << "," << result_x - << "," << result_y << std::endl; + const auto multi_ndt_score_result_x = multi_ndt_score_result.pose(0, 3); + const auto multi_ndt_score_result_y = multi_ndt_score_result.pose(1, 3); + ofs_mndt_score << j << "," << nvtl << "," << initial_x << "," << initial_y << "," + << multi_ndt_score_result_x << "," << multi_ndt_score_result_y << std::endl; } } } diff --git a/apps/pcd_map_grid_manager.hpp b/apps/pcd_map_grid_manager.hpp index a58d0a8..68a079a 100644 --- a/apps/pcd_map_grid_manager.hpp +++ b/apps/pcd_map_grid_manager.hpp @@ -54,12 +54,12 @@ class MapGridManager const int y_min = static_cast(std::ceil((y - get_around_) / resolution_)); const int y_max = static_cast(std::ceil((y + get_around_) / resolution_)); std::vector> curr_keys; - for (int x = x_min; x <= x_max; x++) { - for (int y = y_min; y <= y_max; y++) { - if (map_grid_.count(std::make_pair(x, y)) == 0) { + for (int x_ind = x_min; x_ind <= x_max; x_ind++) { + for (int y_ind = y_min; y_ind <= y_max; y_ind++) { + if (map_grid_.count(std::make_pair(x_ind, y_ind)) == 0) { continue; } - curr_keys.emplace_back(std::make_pair(x, y)); + curr_keys.emplace_back(std::make_pair(x_ind, y_ind)); } } From b0c4075f3c6006f58e681b5c925185cca4dae93e Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 6 Aug 2024 13:11:53 +0900 Subject: [PATCH 4/9] add cppcheck-suppress for redundantAssignment Signed-off-by: a-maumau --- apps/align.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/apps/align.cpp b/apps/align.cpp index 295498a..0e18f4b 100644 --- a/apps/align.cpp +++ b/apps/align.cpp @@ -79,18 +79,21 @@ int main(int argc, char ** argv) std::cout << "--- pcl::GICP ---" << std::endl; pcl::GeneralizedIterativeClosestPoint::Ptr gicp( new pcl::GeneralizedIterativeClosestPoint()); - align(gicp, target_cloud, source_cloud); + // cppcheck-suppress redundantAssignment + aligned = align(gicp, target_cloud, source_cloud); std::cout << "--- pclomp::GICP ---" << std::endl; pclomp::GeneralizedIterativeClosestPoint::Ptr gicp_omp( new pclomp::GeneralizedIterativeClosestPoint()); + // cppcheck-suppress redundantAssignment aligned = align(gicp_omp, target_cloud, source_cloud); std::cout << "--- pcl::NDT ---" << std::endl; pcl::NormalDistributionsTransform::Ptr ndt( new pcl::NormalDistributionsTransform()); ndt->setResolution(1.0); - align(ndt, target_cloud, source_cloud); + // cppcheck-suppress redundantAssignment + aligned = align(ndt, target_cloud, source_cloud); std::vector num_threads = {1, omp_get_max_threads()}; std::vector> search_methods = { @@ -110,11 +113,13 @@ int main(int argc, char ** argv) << std::endl; ndt_omp->setNumThreads(n); ndt_omp->setNeighborhoodSearchMethod(search_method.second); - align(ndt_omp, target_cloud, source_cloud); + // cppcheck-suppress redundantAssignment + aligned = align(ndt_omp, target_cloud, source_cloud); } std::cout << "--- multigrid_pclomp::NDT (" << n << " threads) ---" << std::endl; mg_ndt_omp->setNumThreads(n); + // cppcheck-suppress redundantAssignment aligned = align(mg_ndt_omp, target_cloud, source_cloud); } From e71bb768ffcbf1b24f89769cab4bfb20f08e95a3 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 6 Aug 2024 13:16:42 +0900 Subject: [PATCH 5/9] fix to correct code Signed-off-by: a-maumau --- include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h index 2121c97..bfd60dd 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h +++ b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h @@ -329,6 +329,10 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { sync(); + if (thread_num <= 0) { + thread_num = 1; + } + thread_num_ = thread_num; thread_futs_.resize(thread_num_); processing_inputs_.resize(thread_num_); From 225f568cee076aae02072d7d54ce9d2a4bbf5314 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 6 Aug 2024 13:20:32 +0900 Subject: [PATCH 6/9] apply suggestion Signed-off-by: a-maumau --- .../multigrid_ndt_omp_impl.hpp | 31 +++++++------------ 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index d337d3f..1230fa6 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -194,12 +194,9 @@ MultiGridNormalDistributionsTransform< params_.regularization_scale_factor = 0.0f; params_.use_line_search = false; - double gauss_c1 = NAN; - double gauss_c2 = NAN; - // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] - gauss_c1 = 10.0 * (1 - outlier_ratio_); - gauss_c2 = outlier_ratio_ / pow(params_.resolution, 3); + double gauss_c1 = 10.0 * (1 - outlier_ratio_); + double gauss_c2 = outlier_ratio_ / pow(params_.resolution, 3); gauss_d3_ = -log(gauss_c2); gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3_; gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3_) / gauss_d1_); @@ -213,12 +210,9 @@ void MultiGridNormalDistributionsTransform::computeTra nr_iterations_ = 0; converged_ = false; - double gauss_c1 = NAN; - double gauss_c2 = NAN; - // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] - gauss_c1 = 10 * (1 - outlier_ratio_); - gauss_c2 = outlier_ratio_ / pow(params_.resolution, 3); + double gauss_c1 = 10 * (1 - outlier_ratio_); + double gauss_c2 = outlier_ratio_ / pow(params_.resolution, 3); gauss_d3_ = -log(gauss_c2); gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3_; gauss_d2_ = -2.0 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3_) / gauss_d1_); @@ -252,7 +246,6 @@ void MultiGridNormalDistributionsTransform::computeTra Eigen::Matrix hessian; double score = 0; - double delta_p_norm = NAN; if (regularization_pose_) { Eigen::Transform regularization_pose_transformation; @@ -275,7 +268,7 @@ void MultiGridNormalDistributionsTransform::computeTra delta_p = sv.solve(-score_gradient); // Calculate step length with guaranteed sufficient decrease [More, Thuente 1994] - delta_p_norm = delta_p.norm(); + double delta_p_norm = delta_p.norm(); if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) { if (input_->empty()) { @@ -506,12 +499,12 @@ void MultiGridNormalDistributionsTransform::computeAng Eigen::Matrix & p, bool compute_hessian) { // Simplified math for near 0 angles - double cx = NAN; - double cy = NAN; - double cz = NAN; - double sx = NAN; - double sy = NAN; - double sz = NAN; + double cx = 1.0; + double cy = 1.0; + double cz = 1.0; + double sx = 0.0; + double sy = 0.0; + double sz = 0.0; if (fabs(p(3)) < 10e-5) { // p(3) = 0; cx = 1.0; @@ -874,7 +867,7 @@ double MultiGridNormalDistributionsTransform::trialVal // Equation 2.4.5 [Sun, Yuan 2006] double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; - double a_t_next = NAN; + double a_t_next = 0.0; if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) a_t_next = a_c; From c5123d77c73c63d54b40e3d9ce5076be58a19fd9 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 6 Aug 2024 13:54:46 +0900 Subject: [PATCH 7/9] apply update in .cppcheck_suppressions Signed-off-by: a-maumau --- include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp | 6 +++--- include/pclomp/gicp_omp_impl.hpp | 5 ++--- include/pclomp/voxel_grid_covariance_omp_impl.hpp | 6 +++--- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index 1230fa6..9eda487 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -730,7 +730,7 @@ void MultiGridNormalDistributionsTransform::computeHes // Equations 6.18 and 6.20 [Magnusson 2009] computePointDerivatives(x, point_gradient, point_hessian); - for (auto & cell : neighborhood) { + for (const auto & cell : neighborhood) { // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, // respectively [Magnusson 2009] updateHessian( @@ -740,7 +740,7 @@ void MultiGridNormalDistributionsTransform::computeHes } // Sum over t_hessians - for (auto & th : t_hessians) { + for (const auto & th : t_hessians) { hessian += th; } } @@ -1098,7 +1098,7 @@ MultiGridNormalDistributionsTransform::calculateTransf } // Sum the point-wise scores - for (auto & ts : t_scores) { + for (const auto & ts : t_scores) { score += ts; } diff --git a/include/pclomp/gicp_omp_impl.hpp b/include/pclomp/gicp_omp_impl.hpp index 2ff8b5e..8a1409f 100644 --- a/include/pclomp/gicp_omp_impl.hpp +++ b/include/pclomp/gicp_omp_impl.hpp @@ -227,7 +227,6 @@ void pclomp::GeneralizedIterativeClosestPoint:: int inner_iterations_ = 0; int result = bfgs.minimizeInit(x); - result = BFGSSpace::Running; do { inner_iterations_++; result = bfgs.minimizeOneStep(x); @@ -467,9 +466,9 @@ pclomp::GeneralizedIterativeClosestPoint::computeTrans Eigen::Matrix4f & M_ = mahalanobis_[i]; M_.setZero(); - Eigen::Matrix3d M = M_.block<3, 3>(0, 0).cast(); + // Eigen::Matrix3d M = M_.block<3, 3>(0, 0).cast(); // M = R*C1 - M = R * C1; + Eigen::Matrix3d M = R * C1; // temp = M*R' + C2 = R*C1*R' + C2 Eigen::Matrix3d temp = M * R.transpose(); temp += C2; diff --git a/include/pclomp/voxel_grid_covariance_omp_impl.hpp b/include/pclomp/voxel_grid_covariance_omp_impl.hpp index 9243e2b..7d05705 100644 --- a/include/pclomp/voxel_grid_covariance_omp_impl.hpp +++ b/include/pclomp/voxel_grid_covariance_omp_impl.hpp @@ -130,8 +130,8 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // viewpoint first... if (!filter_field_name_.empty()) { // Get the distance field index - std::vector fields; - int distance_idx = pcl::getFieldIndex(filter_field_name_, fields); + std::vector distance_fields; + int distance_idx = pcl::getFieldIndex(filter_field_name_, distance_fields); if (distance_idx == -1) PCL_WARN( "[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName().c_str(), @@ -149,7 +149,7 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // Get the distance value const auto * pt_data = reinterpret_cast(&input_->points[cp]); float distance_value = 0; - memcpy(&distance_value, pt_data + fields[distance_idx].offset, sizeof(float)); + memcpy(&distance_value, pt_data + distance_fields[distance_idx].offset, sizeof(float)); if (filter_limit_negative_) { // Use a threshold for cutting out points which inside the interval From 33810fd2c8ce472c0bdfaef673651636a72ab9ab Mon Sep 17 00:00:00 2001 From: a-maumau Date: Wed, 7 Aug 2024 14:10:27 +0900 Subject: [PATCH 8/9] add temporal compatibility Signed-off-by: a-maumau --- src/estimate_covariance/estimate_covariance.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/estimate_covariance/estimate_covariance.cpp b/src/estimate_covariance/estimate_covariance.cpp index 120d66f..c49d1c3 100644 --- a/src/estimate_covariance/estimate_covariance.cpp +++ b/src/estimate_covariance/estimate_covariance.cpp @@ -14,6 +14,18 @@ Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( return covariance_xy; } +// temporal compatibility until +// https://github.com/autowarefoundation/autoware.universe/pull/8124 +// get merged +Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation( + const Eigen::Matrix & hessian) +{ + const Eigen::Matrix2d hessian_xy = hessian.block<2, 2>(0, 0); + Eigen::Matrix2d covariance_xy = -hessian_xy.inverse(); + return covariance_xy; +} + + ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, const std::shared_ptr< From 261ea06d9bcbda9a6293c5d7062147ec2e4eb7da Mon Sep 17 00:00:00 2001 From: a-maumau Date: Wed, 7 Aug 2024 14:55:02 +0900 Subject: [PATCH 9/9] add temporal compatibility Signed-off-by: a-maumau --- include/estimate_covariance/estimate_covariance.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/estimate_covariance/estimate_covariance.hpp b/include/estimate_covariance/estimate_covariance.hpp index 35da2a7..4de5bd4 100644 --- a/include/estimate_covariance/estimate_covariance.hpp +++ b/include/estimate_covariance/estimate_covariance.hpp @@ -23,6 +23,11 @@ struct ResultOfMultiNdtCovarianceEstimation /** \brief Estimate functions */ Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( const Eigen::Matrix & hessian); +// temporal compatibility until +// https://github.com/autowarefoundation/autoware.universe/pull/8124 +// get merged +Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation( + const Eigen::Matrix & hessian); ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, const std::shared_ptr<