Memory usage for matching cost factors

include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp

@@ -93,6 +93,11 @@ void IntegratedColorConsistencyFactor_<TargetFrame, SourceFrame, IntensityGradie
   std::cout << "num_threads=" << num_threads << ", max_corr_dist=" << std::sqrt(max_correspondence_distance_sq) << std::endl;
 }
 
+template <typename TargetFrame, typename SourceFrame, typename IntensityGradients>
+size_t IntegratedColorConsistencyFactor_<TargetFrame, SourceFrame, IntensityGradients>::memory_usage() const {
+  return sizeof(*this) + sizeof(long) * correspondences.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame, typename IntensityGradients>
 void IntegratedColorConsistencyFactor_<TargetFrame, SourceFrame, IntensityGradients>::update_correspondences(const Eigen::Isometry3d& delta) const {
   bool do_update = true;

include/gtsam_points/factors/impl/integrated_colored_gicp_factor_impl.hpp

@@ -92,6 +92,11 @@ void IntegratedColoredGICPFactor_<TargetFrame, SourceFrame, IntensityGradients>:
   std::cout << "num_threads=" << num_threads << ", max_corr_dist=" << std::sqrt(max_correspondence_distance_sq) << std::endl;
 }
 
+template <typename TargetFrame, typename SourceFrame, typename IntensityGradients>
+size_t IntegratedColoredGICPFactor_<TargetFrame, SourceFrame, IntensityGradients>::memory_usage() const {
+  return sizeof(*this) + sizeof(long) * correspondences.capacity() + sizeof(Eigen::Matrix4d) * mahalanobis.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame, typename IntensityGradients>
 void IntegratedColoredGICPFactor_<TargetFrame, SourceFrame, IntensityGradients>::update_correspondences(const Eigen::Isometry3d& delta) const {
   bool do_update = true;

include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp

@@ -55,6 +55,11 @@ void IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::print(const std::string
   std::cout << "num_threads=" << this->num_threads << ", max_corr_dist=" << std::sqrt(this->max_correspondence_distance_sq) << std::endl;
 }
 
+template <typename TargetFrame, typename SourceFrame>
+size_t IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::memory_usage() const {
+  return IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::memory_usage() + sizeof(Eigen::Matrix4d) * mahalanobis.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame>
 double IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Values& values) const {
   this->update_poses(values);

include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp

@@ -81,6 +81,13 @@ void IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::print(const std::string&
   std::cout << "num_threads=" << num_threads << ", max_corr_dist=" << std::sqrt(max_correspondence_distance_sq) << std::endl;
 }
 
+template <typename TargetFrame, typename SourceFrame>
+size_t IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::memory_usage() const {
+  return sizeof(*this) + sizeof(double) * time_table.capacity() + sizeof(gtsam::Pose3) * source_poses.capacity() +
+         sizeof(gtsam::Matrix6) * pose_derivatives_t0.capacity() + sizeof(gtsam::Matrix6) * pose_derivatives_t1.capacity() +
+         sizeof(long) * correspondences.capacity() + sizeof(int) * time_indices.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame>
 double IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Values& values) const {
   update_poses(values);

include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp

@@ -117,6 +117,17 @@ void IntegratedGICPFactor_<TargetFrame, SourceFrame>::print(const std::string& s
             << ", cache_mode=" << static_cast<int>(mahalanobis_cache_mode) << std::endl;
 }
 
+/**
+ * @brief  Calculate the memory usage of this factor
+ * @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
+ * @return Memory usage in bytes (Approximate size in bytes)
+ */
+template <typename TargetFrame, typename SourceFrame>
+size_t IntegratedGICPFactor_<TargetFrame, SourceFrame>::memory_usage() const {
+  return sizeof(*this) + sizeof(long) * correspondences.capacity() + sizeof(Eigen::Matrix4d) * mahalanobis_full.capacity() +
+         sizeof(Eigen::Matrix<float, 6, 1>) * mahalanobis_compact.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame>
 void IntegratedGICPFactor_<TargetFrame, SourceFrame>::update_correspondences(const Eigen::Isometry3d& delta) const {
   linearization_point = delta;

include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp

@@ -119,6 +119,11 @@ void IntegratedICPFactor_<TargetFrame, SourceFrame>::print(const std::string& s,
   std::cout << "num_threads=" << num_threads << ", max_corr_dist=" << std::sqrt(max_correspondence_distance_sq) << std::endl;
 }
 
+template <typename TargetFrame, typename SourceFrame>
+size_t IntegratedICPFactor_<TargetFrame, SourceFrame>::memory_usage() const {
+  return sizeof(*this) + sizeof(long) * correspondences.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame>
 void IntegratedICPFactor_<TargetFrame, SourceFrame>::update_correspondences(const Eigen::Isometry3d& delta) const {
   bool do_update = true;

include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp

@@ -71,6 +71,11 @@ void IntegratedPointToPlaneFactor_<TargetFrame, SourceFrame>::print(const std::s
   std::cout << "num_threads=" << num_threads << ", max_corr_dist=" << std::sqrt(max_correspondence_distance_sq) << std::endl;
 }
 
+template <typename TargetFrame, typename SourceFrame>
+size_t IntegratedPointToPlaneFactor_<TargetFrame, SourceFrame>::memory_usage() const {
+  return sizeof(*this) + sizeof(std::tuple<long, long, long>) * correspondences.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame>
 void IntegratedPointToPlaneFactor_<TargetFrame, SourceFrame>::update_correspondences(const Eigen::Isometry3d& delta) const {
   if (correspondences.size() == frame::size(*source) && (correspondence_update_tolerance_trans > 0.0 || correspondence_update_tolerance_rot > 0.0)) {
@@ -286,6 +291,11 @@ void IntegratedPointToEdgeFactor_<TargetFrame, SourceFrame>::print(const std::st
   std::cout << "num_threads=" << num_threads << ", max_corr_dist=" << std::sqrt(max_correspondence_distance_sq) << std::endl;
 }
 
+template <typename TargetFrame, typename SourceFrame>
+size_t IntegratedPointToEdgeFactor_<TargetFrame, SourceFrame>::memory_usage() const {
+  return sizeof(*this) + sizeof(std::tuple<long, long>) * correspondences.capacity();
+}
+
 template <typename TargetFrame, typename SourceFrame>
 double IntegratedPointToEdgeFactor_<TargetFrame, SourceFrame>::evaluate(
   const Eigen::Isometry3d& delta,
@@ -408,6 +418,11 @@ void IntegratedLOAMFactor_<TargetFrame, SourceFrame>::print(const std::string& s
   }
 }
 
+template <typename TargetFrame, typename SourceFrame>
+size_t IntegratedLOAMFactor_<TargetFrame, SourceFrame>::memory_usage() const {
+  return edge_factor->memory_usage() + plane_factor->memory_usage();
+}
+
 template <typename TargetFrame, typename SourceFrame>
 void IntegratedLOAMFactor_<TargetFrame, SourceFrame>::set_num_threads(int n) {
   edge_factor->set_num_threads(n);

include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp

@@ -89,6 +89,12 @@ void IntegratedVGICPFactor_<SourceFrame>::print(const std::string& s, const gtsa
   std::cout << "num_threads=" << num_threads << std::endl;
 }
 
+template <typename SourceFrame>
+size_t IntegratedVGICPFactor_<SourceFrame>::memory_usage() const {
+  return sizeof(*this) + sizeof(const GaussianVoxel*) * correspondences.capacity() + sizeof(Eigen::Matrix4d) * mahalanobis_full.capacity() +
+         sizeof(Eigen::Matrix<float, 6, 1>) * mahalanobis_compact.capacity();
+}
+
 template <typename SourceFrame>
 void IntegratedVGICPFactor_<SourceFrame>::update_correspondences(const Eigen::Isometry3d& delta) const {
   linearization_point = delta;

include/gtsam_points/factors/integrated_color_consistency_factor.hpp

@@ -74,6 +74,8 @@ class IntegratedColorConsistencyFactor_ : public gtsam_points::IntegratedMatchin
   /// @brief Print the factor information.
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  virtual size_t memory_usage() const override;
+
   /// @brief Set the number of thread used for linearization of this factor.
   /// @note If your GTSAM is built with TBB, linearization is already multi-threaded
   ///       and setting n>1 can rather affect the processing speed.

include/gtsam_points/factors/integrated_colored_gicp_factor.hpp

@@ -56,6 +56,8 @@ class IntegratedColoredGICPFactor_ : public gtsam_points::IntegratedMatchingCost
 
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  virtual size_t memory_usage() const override;
+
   void set_num_threads(int n) { num_threads = n; }
   void set_max_correspondence_distance(double d) { max_correspondence_distance_sq = d * d; }
   void set_photometric_term_weight(double w) { photometric_term_weight = w; }

include/gtsam_points/factors/integrated_ct_gicp_factor.hpp

@@ -51,6 +51,8 @@ class IntegratedCT_GICPFactor_ : public IntegratedCT_ICPFactor_<TargetFrame, Sou
   /// @brief Print the factor information.
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  virtual size_t memory_usage() const override;
+
   virtual double error(const gtsam::Values& values) const override;
   virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& values) const override;
 

include/gtsam_points/factors/integrated_ct_icp_factor.hpp

@@ -55,6 +55,8 @@ class IntegratedCT_ICPFactor_ : public gtsam::NonlinearFactor {
   /// @brief Print the factor information.
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  virtual size_t memory_usage() const;
+
   virtual size_t dim() const override { return 6; }
   virtual double error(const gtsam::Values& values) const override;
   virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& values) const override;

include/gtsam_points/factors/integrated_gicp_factor.hpp

@@ -81,6 +81,13 @@ class IntegratedGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor
   /// @brief Print the factor information.
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  /**
+   * @brief  Calculate the memory usage of this factor
+   * @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
+   * @return Memory usage in bytes (Approximate size in bytes)
+   */
+  virtual size_t memory_usage() const override;
+
   /// @brief Set the number of thread used for linearization of this factor.
   /// @note If your GTSAM is built with TBB, linearization is already multi-threaded
   ///       and setting n>1 can rather affect the processing speed.

include/gtsam_points/factors/integrated_icp_factor.hpp

@@ -78,6 +78,13 @@ class IntegratedICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor {
   /// @brief Print the factor information.
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  /**
+   * @brief  Calculate the memory usage of this factor
+   * @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
+   * @return Memory usage in bytes (Approximate size in bytes)
+   */
+  virtual size_t memory_usage() const override;
+
   /// @brief Set the number of thread used for linearization of this factor.
   /// @note If your GTSAM is built with TBB, linearization is already multi-threaded
   ///       and setting n>1 can rather affect the processing speed.

include/gtsam_points/factors/integrated_loam_factor.hpp

@@ -53,6 +53,8 @@ class IntegratedLOAMFactor_ : public gtsam_points::IntegratedMatchingCostFactor
 
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  virtual size_t memory_usage() const override;
+
   // note: If your GTSAM is built with TBB, linearization is already multi-threaded
   //     : and setting n>1 can rather affect the processing speed
   void set_num_threads(int n);
@@ -109,6 +111,8 @@ class IntegratedPointToPlaneFactor_ : public gtsam_points::IntegratedMatchingCos
 
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  virtual size_t memory_usage() const override;
+
   void set_num_threads(int n) { num_threads = n; }
   void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; }
   void set_correspondence_update_tolerance(double angle, double trans) {
@@ -169,6 +173,8 @@ class IntegratedPointToEdgeFactor_ : public gtsam_points::IntegratedMatchingCost
 
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  virtual size_t memory_usage() const override;
+
   void set_num_threads(int n) { num_threads = n; }
   void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; }
   void set_correspondence_update_tolerance(double angle, double trans) {

include/gtsam_points/factors/integrated_matching_cost_factor.hpp

@@ -50,6 +50,13 @@ class IntegratedMatchingCostFactor : public gtsam::NonlinearFactor {
 public:
   Eigen::Isometry3d calc_delta(const gtsam::Values& values) const;
 
+  /**
+   * @brief  Calculate the memory usage of this factor
+   * @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
+   * @return Memory usage in bytes (Approximate size in bytes)
+   */
+  virtual size_t memory_usage() const;
+
   /**
    * @brief Update point correspondences
    * @param delta Transformation between target and source (i.e., T_target_source)

include/gtsam_points/factors/integrated_vgicp_factor.hpp

@@ -57,6 +57,13 @@ class IntegratedVGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor
   /// @brief Print the factor information.
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  /**
+   * @brief  Calculate the memory usage of this factor
+   * @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
+   * @return Memory usage in bytes (Approximate size in bytes)
+   */
+  virtual size_t memory_usage() const override;
+
   /// @brief Set the number of thread used for linearization of this factor.
   /// @note If your GTSAM is built with TBB, linearization is already multi-threaded
   ///       and setting n>1 can rather affect the processing speed.

include/gtsam_points/factors/integrated_vgicp_factor_gpu.hpp

@@ -68,6 +68,16 @@ class IntegratedVGICPFactorGPU : public gtsam_points::NonlinearFactorGPU {
   /// @brief Print the factor information.
   virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
+  /// @brief  Calculate the CPU memory usage of this factor
+  /// @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
+  /// @return Approximate CPU memory usage in bytes
+  size_t memory_usage() const;
+
+  /// @brief  Calculate the GPU memory usage of this factor
+  /// @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
+  /// @return Approximate GPU memory usage in bytes
+  size_t memory_usage_gpu() const;
+
   /// @brief Enable or disable surface orientation validation for correspondence search
   /// @note  To enable surface orientation validation, source frame must have point normals
   void set_enable_surface_validation(bool enable);

src/gtsam_points/factors/integrated_matching_cost_factor.cpp

@@ -68,4 +68,8 @@ Eigen::Isometry3d IntegratedMatchingCostFactor::calc_delta(const gtsam::Values&
   }
 }
 
+size_t IntegratedMatchingCostFactor::memory_usage() const {
+  return sizeof(*this);
+}
+
 }  // namespace gtsam_points

src/gtsam_points/factors/integrated_vgicp_factor_gpu.cpp

@@ -91,6 +91,14 @@ void IntegratedVGICPFactorGPU::print(const std::string& s, const gtsam::KeyForma
   std::cout << "target_resolusion=" << target->voxel_resolution() << ", |source|=" << frame::size(*source) << "pts" << std::endl;
 }
 
+size_t IntegratedVGICPFactorGPU::memory_usage() const {
+  return sizeof(*this) + sizeof(IntegratedVGICPDerivatives);
+}
+
+size_t IntegratedVGICPFactorGPU::memory_usage_gpu() const {
+  return sizeof(Eigen::Isometry3f) + sizeof(int) + sizeof(int) * derivatives->get_num_inliers();
+}
+
 void IntegratedVGICPFactorGPU::set_enable_surface_validation(bool enable) {
   derivatives->set_enable_surface_validation(enable);
 }

src/test/test_global_registration.cpp

@@ -86,7 +86,7 @@ TEST_P(GlobalRegistrationTest, RegistrationTest) {
 
   if (method == "RANSAC") {
     gtsam_points::RANSACParams params;
-    params.num_threads = 2;
+    params.num_threads = 1;
     params.dof = dof;
     result = gtsam_points::estimate_pose_ransac(
       *target,
@@ -98,7 +98,7 @@ TEST_P(GlobalRegistrationTest, RegistrationTest) {
       params);
   } else {
     gtsam_points::GNCParams params;
-    params.num_threads = 2;
+    params.num_threads = 1;
     params.dof = dof;
     result = gtsam_points::estimate_pose_gnc(
       *target,
@@ -114,12 +114,12 @@ TEST_P(GlobalRegistrationTest, RegistrationTest) {
   const Eigen::Isometry3d error = T_target_source.inverse() * result.T_target_source;
   const double error_t = error.translation().norm();
   const double error_r = Eigen::AngleAxisd(error.linear()).angle();
-  EXPECT_LE(error_t, 0.5);
-  EXPECT_LE(error_r, 0.1);
+  EXPECT_LE(error_t, 0.5) << "Large translation error in " << method << " registration";
+  EXPECT_LE(error_r, 0.1) << "Large rotation error in " << method << " registration";
 
   if (dof == 4) {
     const Eigen::Vector3d z = result.T_target_source.linear().col(2);
-    EXPECT_NEAR((z - Eigen::Vector3d::UnitZ()).cwiseAbs().maxCoeff(), 0.0, 1e-6);
+    EXPECT_NEAR((z - Eigen::Vector3d::UnitZ()).cwiseAbs().maxCoeff(), 0.0, 1e-6) << "Non-vertical z-axis in 4DoF registration";
   }
 }