implemented clone method for integrated_(v)gicp and rotate_vector3 fa…

…ctors
koide3 · Jan 24, 2025 · fef5953 · fef5953
1 parent b68b388
commit fef5953
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 3 deletions.
diff --git a/include/gtsam_points/factors/integrated_gicp_factor.hpp b/include/gtsam_points/factors/integrated_gicp_factor.hpp
@@ -104,6 +104,8 @@ class IntegratedGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor
     return static_cast<double>(inliers) / correspondences.size();
   }
 
+  gtsam::NonlinearFactor::shared_ptr clone() const override { return gtsam::NonlinearFactor::shared_ptr(new IntegratedGICPFactor_(*this)); }
+
 private:
   virtual void update_correspondences(const Eigen::Isometry3d& delta) const override;
 

diff --git a/include/gtsam_points/factors/integrated_vgicp_factor.hpp b/include/gtsam_points/factors/integrated_vgicp_factor.hpp
@@ -69,6 +69,14 @@ class IntegratedVGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor
     return static_cast<double>(inliers) / correspondences.size();
   }
 
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    if (is_binary) {
+      return gtsam::make_shared<IntegratedVGICPFactor_>(keys()[0], keys()[1], target_voxels, source);
+    }
+
+    return gtsam::make_shared<IntegratedVGICPFactor_>(gtsam::Pose3(fixed_target_pose.cast<double>().matrix()), keys()[0], target_voxels, source);
+  }
+
 private:
   virtual void update_correspondences(const Eigen::Isometry3d& delta) const override;
 

diff --git a/include/gtsam_points/factors/rotate_vector3_factor.hpp b/include/gtsam_points/factors/rotate_vector3_factor.hpp
@@ -16,11 +16,17 @@ class RotateVector3Factor : public gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam:
   using Base = gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam::Vector3>;
 
   RotateVector3Factor() {}
-  RotateVector3Factor(gtsam::Key x, gtsam::Key v, const gtsam::Vector3& local_v, const gtsam::SharedNoiseModel& noise_model) : Base(noise_model, x, v), local_v(local_v) {}
+  RotateVector3Factor(gtsam::Key x, gtsam::Key v, const gtsam::Vector3& local_v, const gtsam::SharedNoiseModel& noise_model)
+  : Base(noise_model, x, v),
+    local_v(local_v) {}
 
   virtual ~RotateVector3Factor() {}
 
-  virtual gtsam::Vector evaluateError(const gtsam::Pose3& x, const gtsam::Vector3& v, boost::optional<gtsam::Matrix&> H_x, boost::optional<gtsam::Matrix&> H_v) const override {
+  virtual gtsam::Vector evaluateError(
+    const gtsam::Pose3& x,
+    const gtsam::Vector3& v,
+    boost::optional<gtsam::Matrix&> H_x,
+    boost::optional<gtsam::Matrix&> H_v) const override {
     gtsam::Matrix36 H_x1;
     gtsam::Matrix33 H_x2;
     gtsam::Vector3 v_ = x.rotation(H_x1).rotate(local_v, H_x2);
@@ -37,6 +43,10 @@ class RotateVector3Factor : public gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam:
     return error;
   }
 
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return gtsam::NonlinearFactor::shared_ptr(new RotateVector3Factor(*this));
+  }
+
 private:
   /** Serialization function */
   friend class boost::serialization::access;
@@ -50,4 +60,4 @@ class RotateVector3Factor : public gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam:
   gtsam::Vector3 local_v;
 };
 
-}  // namespace gtsam
+}  // namespace gtsam_points