From ff8269ec09c9ddccff8358a4b7cc5e4c5fda134f Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Mon, 8 Apr 2024 17:58:50 +0900 Subject: [PATCH] static code analysis (#20) --- include/small_gicp/ann/incremental_voxelmap.hpp | 2 +- include/small_gicp/ann/kdtree.hpp | 10 ++++++---- include/small_gicp/benchmark/benchmark.hpp | 2 +- include/small_gicp/factors/plane_icp_factor.hpp | 6 +++--- include/small_gicp/points/point_cloud.hpp | 2 +- include/small_gicp/registration/optimizer.hpp | 4 ++++ include/small_gicp/util/lie.hpp | 4 +--- src/benchmark/odometry_benchmark_fast_gicp.cpp | 2 +- src/benchmark/odometry_benchmark_fast_vgicp.cpp | 2 +- src/benchmark/odometry_benchmark_pcl.cpp | 2 +- src/benchmark/odometry_benchmark_small_gicp.cpp | 2 +- .../odometry_benchmark_small_gicp_model_tbb.cpp | 2 +- src/benchmark/odometry_benchmark_small_gicp_omp.cpp | 2 +- src/benchmark/odometry_benchmark_small_gicp_pcl.cpp | 2 +- src/benchmark/odometry_benchmark_small_gicp_tbb.cpp | 2 +- .../odometry_benchmark_small_gicp_tbb_flow.cpp | 2 +- .../odometry_benchmark_small_vgicp_model_tbb.cpp | 2 +- src/benchmark/odometry_benchmark_small_vgicp_omp.cpp | 2 +- src/benchmark/odometry_benchmark_small_vgicp_tbb.cpp | 2 +- 19 files changed, 29 insertions(+), 25 deletions(-) diff --git a/include/small_gicp/ann/incremental_voxelmap.hpp b/include/small_gicp/ann/incremental_voxelmap.hpp index 180797b..37f7b71 100644 --- a/include/small_gicp/ann/incremental_voxelmap.hpp +++ b/include/small_gicp/ann/incremental_voxelmap.hpp @@ -40,7 +40,7 @@ struct IncrementalVoxelMap { /// @brief Constructor /// @param leaf_size Voxel size - IncrementalVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {} + explicit IncrementalVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {} /// @brief Number of points in the voxelmap size_t size() const { return flat_voxels.size(); } diff --git a/include/small_gicp/ann/kdtree.hpp b/include/small_gicp/ann/kdtree.hpp index 2318068..5b1c611 100644 --- a/include/small_gicp/ann/kdtree.hpp +++ b/include/small_gicp/ann/kdtree.hpp @@ -23,12 +23,14 @@ class UnsafeKdTreeGeneric { /// @brief Constructor /// @param points Input points - UnsafeKdTreeGeneric(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); } + explicit UnsafeKdTreeGeneric(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); } /// @brief Constructor /// @param points Input points /// @params num_threads Number of threads used for building the index (This argument is only valid for OMP implementation) - UnsafeKdTreeGeneric(const PointCloud& points, int num_threads) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(num_threads); } + explicit UnsafeKdTreeGeneric(const PointCloud& points, int num_threads) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { + index.buildIndex(num_threads); + } ~UnsafeKdTreeGeneric() {} @@ -58,11 +60,11 @@ class KdTreeGeneric { /// @brief Constructor /// @param points Input points - KdTreeGeneric(const std::shared_ptr& points) : points(points), tree(*points) {} + explicit KdTreeGeneric(const std::shared_ptr& points) : points(points), tree(*points) {} /// @brief Constructor /// @param points Input points - KdTreeGeneric(const std::shared_ptr& points, int num_threads) : points(points), tree(*points, num_threads) {} + explicit KdTreeGeneric(const std::shared_ptr& points, int num_threads) : points(points), tree(*points, num_threads) {} ~KdTreeGeneric() {} diff --git a/include/small_gicp/benchmark/benchmark.hpp b/include/small_gicp/benchmark/benchmark.hpp index c560a78..b089a24 100644 --- a/include/small_gicp/benchmark/benchmark.hpp +++ b/include/small_gicp/benchmark/benchmark.hpp @@ -95,7 +95,7 @@ std::string summarize(const Container& container, const Transform& transform) { struct KittiDataset { public: - KittiDataset(const std::string& dataset_path, size_t max_num_data = std::numeric_limits::max()) { + explicit KittiDataset(const std::string& dataset_path, size_t max_num_data = 1000000) { std::vector filenames; for (auto path : std::filesystem::directory_iterator(dataset_path)) { if (path.path().extension() != ".bin") { diff --git a/include/small_gicp/factors/plane_icp_factor.hpp b/include/small_gicp/factors/plane_icp_factor.hpp index b0aaf76..150dfcf 100644 --- a/include/small_gicp/factors/plane_icp_factor.hpp +++ b/include/small_gicp/factors/plane_icp_factor.hpp @@ -41,15 +41,15 @@ struct PointToPlaneICPFactor { const auto& target_normal = traits::normal(target, target_index); const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; - const Eigen::Vector4d error = target_normal.array() * residual.array(); + const Eigen::Vector4d err = target_normal.array() * residual.array(); Eigen::Matrix J = Eigen::Matrix::Zero(); J.block<3, 3>(0, 0) = target_normal.template head<3>().asDiagonal() * T.linear() * skew(traits::point(source, source_index).template head<3>()); J.block<3, 3>(0, 3) = target_normal.template head<3>().asDiagonal() * (-T.linear()); *H = J.transpose() * J; - *b = J.transpose() * error; - *e = 0.5 * error.squaredNorm(); + *b = J.transpose() * err; + *e = 0.5 * err.squaredNorm(); return true; } diff --git a/include/small_gicp/points/point_cloud.hpp b/include/small_gicp/points/point_cloud.hpp index 880f06a..5e9091f 100644 --- a/include/small_gicp/points/point_cloud.hpp +++ b/include/small_gicp/points/point_cloud.hpp @@ -23,7 +23,7 @@ struct PointCloud { /// @brief Constructor /// @param points Points to initialize the point cloud template - PointCloud(const std::vector, Allocator>& points) { + explicit PointCloud(const std::vector, Allocator>& points) { this->resize(points.size()); for (size_t i = 0; i < points.size(); i++) { this->point(i) << points[i].template cast().template head<3>(), 1.0; diff --git a/include/small_gicp/registration/optimizer.hpp b/include/small_gicp/registration/optimizer.hpp index 8768420..1962c85 100644 --- a/include/small_gicp/registration/optimizer.hpp +++ b/include/small_gicp/registration/optimizer.hpp @@ -136,6 +136,10 @@ struct LevenbergMarquardtOptimizer { result.H = H; result.b = b; result.error = e; + + if (!success) { + break; + } } result.num_inliers = std::count_if(factors.begin(), factors.end(), [](const auto& factor) { return factor.inlier(); }); diff --git a/include/small_gicp/util/lie.hpp b/include/small_gicp/util/lie.hpp index 7a74bbd..2f4aa8f 100644 --- a/include/small_gicp/util/lie.hpp +++ b/include/small_gicp/util/lie.hpp @@ -50,16 +50,14 @@ inline Eigen::Matrix3d skew(const Eigen::Vector3d& x) { inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { double theta_sq = omega.dot(omega); - double theta; double imag_factor; double real_factor; if (theta_sq < 1e-10) { - theta = 0; double theta_quad = theta_sq * theta_sq; imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; } else { - theta = std::sqrt(theta_sq); + double theta = std::sqrt(theta_sq); double half_theta = 0.5 * theta; imag_factor = std::sin(half_theta) / theta; real_factor = std::cos(half_theta); diff --git a/src/benchmark/odometry_benchmark_fast_gicp.cpp b/src/benchmark/odometry_benchmark_fast_gicp.cpp index 2f948ec..3b63af5 100644 --- a/src/benchmark/odometry_benchmark_fast_gicp.cpp +++ b/src/benchmark/odometry_benchmark_fast_gicp.cpp @@ -14,7 +14,7 @@ namespace small_gicp { class FastGICPOdometryEstimation : public OnlineOdometryEstimation { public: - FastGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { + explicit FastGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { gicp.setCorrespondenceRandomness(params.num_neighbors); gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); gicp.setNumThreads(params.num_threads); diff --git a/src/benchmark/odometry_benchmark_fast_vgicp.cpp b/src/benchmark/odometry_benchmark_fast_vgicp.cpp index e4bb1e0..bf2186a 100644 --- a/src/benchmark/odometry_benchmark_fast_vgicp.cpp +++ b/src/benchmark/odometry_benchmark_fast_vgicp.cpp @@ -16,7 +16,7 @@ namespace small_gicp { class FastVGICPOdometryEstimation : public OnlineOdometryEstimation { public: - FastVGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { + explicit FastVGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { vgicp.setCorrespondenceRandomness(params.num_neighbors); vgicp.setResolution(params.voxel_resolution); vgicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); diff --git a/src/benchmark/odometry_benchmark_pcl.cpp b/src/benchmark/odometry_benchmark_pcl.cpp index 3f56bd5..bcee622 100644 --- a/src/benchmark/odometry_benchmark_pcl.cpp +++ b/src/benchmark/odometry_benchmark_pcl.cpp @@ -10,7 +10,7 @@ namespace small_gicp { class PCLOnlineOdometryEstimation : public OnlineOdometryEstimation { public: - PCLOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { + explicit PCLOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { gicp.setCorrespondenceRandomness(params.num_neighbors); gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); } diff --git a/src/benchmark/odometry_benchmark_small_gicp.cpp b/src/benchmark/odometry_benchmark_small_gicp.cpp index e76c39f..4ee6a48 100644 --- a/src/benchmark/odometry_benchmark_small_gicp.cpp +++ b/src/benchmark/odometry_benchmark_small_gicp.cpp @@ -11,7 +11,7 @@ namespace small_gicp { class SmallGICPOnlineOdometryEstimation : public OnlineOdometryEstimation { public: - SmallGICPOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} + explicit SmallGICPOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { Stopwatch sw; diff --git a/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp b/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp index 7942624..402ff60 100644 --- a/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp +++ b/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp @@ -14,7 +14,7 @@ namespace small_gicp { class SmallGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { public: - SmallGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) + explicit SmallGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), control(tbb::global_control::max_allowed_parallelism, params.num_threads), T_world_lidar(Eigen::Isometry3d::Identity()) {} diff --git a/src/benchmark/odometry_benchmark_small_gicp_omp.cpp b/src/benchmark/odometry_benchmark_small_gicp_omp.cpp index ee10c09..b8c3d10 100644 --- a/src/benchmark/odometry_benchmark_small_gicp_omp.cpp +++ b/src/benchmark/odometry_benchmark_small_gicp_omp.cpp @@ -11,7 +11,7 @@ namespace small_gicp { class SmallGICPOnlineOdometryEstimationOMP : public OnlineOdometryEstimation { public: - SmallGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T_world_lidar(Eigen::Isometry3d::Identity()) {} + explicit SmallGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T_world_lidar(Eigen::Isometry3d::Identity()) {} Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { Stopwatch sw; diff --git a/src/benchmark/odometry_benchmark_small_gicp_pcl.cpp b/src/benchmark/odometry_benchmark_small_gicp_pcl.cpp index 8fee6a9..19d6531 100644 --- a/src/benchmark/odometry_benchmark_small_gicp_pcl.cpp +++ b/src/benchmark/odometry_benchmark_small_gicp_pcl.cpp @@ -12,7 +12,7 @@ namespace small_gicp { class SmallGICPPCLOdometryEstimation : public OnlineOdometryEstimation { public: - SmallGICPPCLOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { + explicit SmallGICPPCLOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { gicp.setCorrespondenceRandomness(params.num_neighbors); gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); gicp.setNumThreads(params.num_threads); diff --git a/src/benchmark/odometry_benchmark_small_gicp_tbb.cpp b/src/benchmark/odometry_benchmark_small_gicp_tbb.cpp index d204856..53bbed7 100644 --- a/src/benchmark/odometry_benchmark_small_gicp_tbb.cpp +++ b/src/benchmark/odometry_benchmark_small_gicp_tbb.cpp @@ -14,7 +14,7 @@ namespace small_gicp { class SmallGICPOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { public: - SmallGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) + explicit SmallGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), control(tbb::global_control::max_allowed_parallelism, params.num_threads), T(Eigen::Isometry3d::Identity()) {} diff --git a/src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp b/src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp index a580f6d..cd76021 100644 --- a/src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp +++ b/src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp @@ -27,7 +27,7 @@ class SmallGICPFlowEstimationTBB : public OdometryEstimation { InputFrame::Ptr source; // Source frame }; - SmallGICPFlowEstimationTBB(const OdometryEstimationParams& params) + explicit SmallGICPFlowEstimationTBB(const OdometryEstimationParams& params) : OdometryEstimation(params), control(tbb::global_control::max_allowed_parallelism, params.num_threads), throughput(0.0) { diff --git a/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp b/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp index 07142bb..269f89a 100644 --- a/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp +++ b/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp @@ -14,7 +14,7 @@ namespace small_gicp { class SmallVGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { public: - SmallVGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) + explicit SmallVGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), control(tbb::global_control::max_allowed_parallelism, params.num_threads), T_world_lidar(Eigen::Isometry3d::Identity()) {} diff --git a/src/benchmark/odometry_benchmark_small_vgicp_omp.cpp b/src/benchmark/odometry_benchmark_small_vgicp_omp.cpp index c647d5f..81de1d0 100644 --- a/src/benchmark/odometry_benchmark_small_vgicp_omp.cpp +++ b/src/benchmark/odometry_benchmark_small_vgicp_omp.cpp @@ -12,7 +12,7 @@ namespace small_gicp { class SmallVGICPOnlineOdometryEstimationOMP : public OnlineOdometryEstimation { public: - SmallVGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} + explicit SmallVGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { Stopwatch sw; diff --git a/src/benchmark/odometry_benchmark_small_vgicp_tbb.cpp b/src/benchmark/odometry_benchmark_small_vgicp_tbb.cpp index e064c18..9dded82 100644 --- a/src/benchmark/odometry_benchmark_small_vgicp_tbb.cpp +++ b/src/benchmark/odometry_benchmark_small_vgicp_tbb.cpp @@ -14,7 +14,7 @@ namespace small_gicp { class SmallVGICPOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { public: - SmallVGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) + explicit SmallVGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), control(tbb::global_control::max_allowed_parallelism, params.num_threads), T(Eigen::Isometry3d::Identity()) {}