refactor(ndt_omp): apply static analysis

apps/align.cpp

@@ -25,13 +25,13 @@ pcl::PointCloud<pcl::PointXYZ>::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<double>((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<double>((t3 - t2).count()) / 1e6 << "[msec]" << std::endl;
   std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl;
 
   return aligned;
@@ -73,21 +73,26 @@ int main(int argc, char ** argv)
   voxelgrid.filter(*downsampled);
   source_cloud = downsampled;
 
+  pcl::PointCloud<pcl::PointXYZ>::Ptr aligned;
+
   // benchmark
   std::cout << "--- pcl::GICP ---" << std::endl;
   pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp(
     new pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
-  pcl::PointCloud<pcl::PointXYZ>::Ptr aligned = align(gicp, target_cloud, source_cloud);
+  // cppcheck-suppress redundantAssignment
+  aligned = align(gicp, target_cloud, source_cloud);
 
   std::cout << "--- pclomp::GICP ---" << std::endl;
   pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp_omp(
     new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
+  // cppcheck-suppress redundantAssignment
   aligned = align(gicp_omp, target_cloud, source_cloud);
 
   std::cout << "--- pcl::NDT ---" << std::endl;
   pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt(
     new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
   ndt->setResolution(1.0);
+  // cppcheck-suppress redundantAssignment
   aligned = align(ndt, target_cloud, source_cloud);
 
   std::vector<int> num_threads = {1, omp_get_max_threads()};
@@ -108,11 +113,13 @@ int main(int argc, char ** argv)
                 << std::endl;
       ndt_omp->setNumThreads(n);
       ndt_omp->setNeighborhoodSearchMethod(search_method.second);
+      // 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);
   }
 

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<int64_t>(initial_pose_list.size());
 
   // prepare ndt
   std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>>
@@ -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<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());
     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<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+      static_cast<double>(std::chrono::duration_cast<std::chrono::microseconds>(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<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+      static_cast<double>(std::chrono::duration_cast<std::chrono::microseconds>(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<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+      static_cast<double>(std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count()) /
+      1000.0;
 
     // output result
     const auto result_x = ndt_result.pose(0, 3);
@@ -174,34 +177,34 @@ 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<int64_t>(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++) {
-      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
     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<int64_t>(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++) {
-      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;
     }
   }
 }

apps/pcd_map_grid_manager.hpp

@@ -16,6 +16,12 @@
 #define NDT_OMP__APPS__PCD_MAP_GRID_MANAGER_HPP_
 
 #include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <map>
+#include <string>
+#include <utility>
+#include <vector>
 
 using AddPair = std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>;
 using AddResult = std::vector<AddPair>;
@@ -24,7 +30,7 @@ using RemoveResult = std::vector<std::string>;
 class MapGridManager
 {
 public:
-  MapGridManager(const pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud)
+  explicit MapGridManager(const pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud)
   : target_cloud_(target_cloud)
   {
     for (const auto & point : target_cloud_->points) {
@@ -48,12 +54,12 @@ class MapGridManager
     const int y_min = static_cast<int>(std::ceil((y - get_around_) / resolution_));
     const int y_max = static_cast<int>(std::ceil((y + get_around_) / resolution_));
     std::vector<std::pair<int, int>> 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.push_back(std::make_pair(x, y));
+        curr_keys.emplace_back(std::make_pair(x_ind, y_ind));
       }
     }
 
@@ -84,7 +90,7 @@ class MapGridManager
   std::map<std::pair<int, int>, pcl::PointCloud<pcl::PointXYZ>::Ptr> map_grid_;
   std::vector<std::pair<int, int>> held_keys_;
 
-  std::string to_string_key(const std::pair<int, int> & key)
+  static std::string to_string_key(const std::pair<int, int> & key)
   {
     return std::to_string(key.first) + "_" + std::to_string(key.second);
   }

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<int64_t>(initial_pose_list.size());
 
   // prepare ndt
   pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr source_cloud = load_pcd(source_pcd);
     mg_ndt_omp->setInputSource(source_cloud);

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<std::chrono::microseconds>(elapsed_time).count();
+    const double microseconds = static_cast<double>(
+      std::chrono::duration_cast<std::chrono::microseconds>(elapsed_time).count());
     return microseconds / 1000.0;
   }
 

apps/util.hpp

@@ -15,20 +15,29 @@
 #ifndef NDT_OMP__APPS__UTIL_HPP_
 #define NDT_OMP__APPS__UTIL_HPP_
 
-std::vector<std::string> glob(const std::string & input_dir)
+#include <glob.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+inline std::vector<std::string> glob(const std::string & input_dir)
 {
   glob_t buffer;
   std::vector<std::string> 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<pcl::PointXYZ>::Ptr load_pcd(const std::string & path)
+inline pcl::PointCloud<pcl::PointXYZ>::Ptr load_pcd(const std::string & path)
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcd(new pcl::PointCloud<pcl::PointXYZ>());
   if (pcl::io::loadPCDFile(path, *pcd)) {
@@ -38,7 +47,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr load_pcd(const std::string & path)
   return pcd;
 }
 
-std::vector<Eigen::Matrix4f> load_pose_list(const std::string & path)
+inline std::vector<Eigen::Matrix4f> 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<Eigen::Matrix4f> 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<float>(quat_w), static_cast<float>(quat_x),
+                               static_cast<float>(quat_y), static_cast<float>(quat_z))
+                               .toRotationMatrix();
+    pose.block<3, 1>(0, 3) = Eigen::Vector3f(
+      static_cast<float>(pose_x), static_cast<float>(pose_y), static_cast<float>(pose_z));
     pose_list.push_back(pose);
   }
   return pose_list;

include/estimate_covariance/estimate_covariance.hpp

@@ -5,6 +5,10 @@
 
 #include <Eigen/Core>
 
+#include <memory>
+#include <utility>
+#include <vector>
+
 namespace pclomp
 {
 
@@ -17,17 +21,22 @@ struct ResultOfMultiNdtCovarianceEstimation
 };
 
 /** \brief Estimate functions */
+Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation(
+  const Eigen::Matrix<double, 6, 6> & 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<double, 6, 6> & hessian);
 ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(
   const NdtResult & ndt_result,
-  std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>>
-    ndt_ptr,
+  const std::shared_ptr<
+    pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
   const std::vector<Eigen::Matrix4f> & poses_to_search);
 ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(
   const NdtResult & ndt_result,
-  std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>>
-    ndt_ptr,
+  const std::shared_ptr<
+    pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
   const std::vector<Eigen::Matrix4f> & poses_to_search, const double temperature);
 
 /** \brief Find rotation matrix aligning covariance to principal axes

include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h

@@ -277,10 +277,10 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT>
   }
 
   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.
    */
@@ -330,7 +330,7 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT>
     sync();
 
     if (thread_num <= 0) {
-      thread_num_ = 1;
+      thread_num = 1;
     }
 
     thread_num_ = thread_num;