Feature/smooth ndt map update

.clang-format

@@ -20,7 +20,7 @@ AlwaysBreakBeforeMultilineStrings: true
 AlwaysBreakTemplateDeclarations: true
 BinPackArguments: true
 BinPackParameters: false
-BraceWrapping:   
+BraceWrapping:
   AfterClass:      false
   AfterControlStatement: false
   AfterEnum:       false
@@ -55,12 +55,12 @@ DerivePointerAlignment: true
 DisableFormat:   false
 ExperimentalAutoDetectBinPacking: false
 FixNamespaceComments: true
-ForEachMacros:   
+ForEachMacros:
   - foreach
   - Q_FOREACH
   - BOOST_FOREACH
 IncludeBlocks:   Preserve
-IncludeCategories: 
+IncludeCategories:
   - Regex:           '^<ext/.*\.h>'
     Priority:        2
   - Regex:           '^<.*\.h>'

.github/workflows/build.yml

@@ -4,6 +4,9 @@ on:
   push:
     branches: [ master ]
   pull_request:
+    types:
+      - opened
+      - synchronize
 
   # Allows you to run this workflow manually from the Actions tab
   workflow_dispatch:

.github/workflows/pre-commit.yml

@@ -0,0 +1,21 @@
+name: pre-commit
+
+on:
+  pull_request:
+    types:
+      - opened
+      - synchronize
+
+jobs:
+  pre-commit:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v4
+        with:
+          ref: ${{ github.event.pull_request.head.ref }}
+
+      - name: Run pre-commit
+        uses: autowarefoundation/autoware-github-actions/pre-commit@v1
+        with:
+          pre-commit-config: .pre-commit-config.yaml

.pre-commit-config.yaml

@@ -0,0 +1,9 @@
+ci:
+  autofix_commit_msg: "style(pre-commit): autofix"
+
+repos:
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v17.0.6
+    hooks:
+      - id: clang-format
+        types_or: [c++, c, cuda]

include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h

@@ -223,6 +223,12 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
     filter_name_ = "MultiVoxelGridCovariance";
   }
 
+  MultiVoxelGridCovariance(const MultiVoxelGridCovariance &other);
+  MultiVoxelGridCovariance(MultiVoxelGridCovariance &&other);
+
+  MultiVoxelGridCovariance &operator=(const MultiVoxelGridCovariance &other);
+  MultiVoxelGridCovariance &operator=(MultiVoxelGridCovariance &&other);
+
   /** \brief Initializes voxel structure.
    */
   inline void setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id) {

include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp

@@ -56,6 +56,62 @@
 #include <pcl/filters/boost.h>
 #include "multigrid_pclomp/multi_voxel_grid_covariance_omp.h"
 
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT>::MultiVoxelGridCovariance(const MultiVoxelGridCovariance &other)
+    : pcl::VoxelGrid<PointT>(other), leaves_(other.leaves_), grid_leaves_(other.grid_leaves_), leaf_indices_(other.leaf_indices_), kdtree_(other.kdtree_) {
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+
+  // Deep copy
+  voxel_centroids_ptr_.reset(new PointCloud);
+
+  if(other.voxel_centroids_ptr_) {
+    *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_;
+  }
+}
+
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT>::MultiVoxelGridCovariance(MultiVoxelGridCovariance &&other)
+    : pcl::VoxelGrid<PointT>(std::move(other)), leaves_(std::move(other.leaves_)), grid_leaves_(std::move(other.grid_leaves_)), leaf_indices_(std::move(other.leaf_indices_)), kdtree_(std::move(other.kdtree_)), voxel_centroids_ptr_(std::move(other.voxel_centroids_ptr_)) {
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+}
+
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT> &pclomp::MultiVoxelGridCovariance<PointT>::operator=(const MultiVoxelGridCovariance &other) {
+  pcl::VoxelGrid<PointT>::operator=(other);
+  leaves_ = other.leaves_;
+  grid_leaves_ = other.grid_leaves_;
+  leaf_indices_ = other.leaf_indices_;
+  kdtree_ = other.kdtree_;
+  voxel_centroids_ptr_ = other.voxel_centroids_ptr_;
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+
+  // Deep copy
+  voxel_centroids_ptr_.reset(new PointCloud);
+
+  if(other.voxel_centroids_ptr_) {
+    *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_;
+  }
+
+  return *this;
+}
+
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT> &pclomp::MultiVoxelGridCovariance<PointT>::operator=(MultiVoxelGridCovariance &&other) {
+  pcl::VoxelGrid<PointT>::operator=(std::move(other));
+  leaves_ = std::move(other.leaves_);
+  grid_leaves_ = std::move(other.grid_leaves_);
+  leaf_indices_ = std::move(other.leaf_indices_);
+  kdtree_ = std::move(other.kdtree_);
+  voxel_centroids_ptr_ = std::move(other.voxel_centroids_ptr_);
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+
+  return *this;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT>
 void pclomp::MultiVoxelGridCovariance<PointT>::applyFilter(const PointCloudConstPtr &input, const std::string &grid_id, LeafDict &leaves) const {

include/multigrid_pclomp/multigrid_ndt_omp.h

@@ -72,6 +72,16 @@ struct NdtResult {
   Eigen::Matrix<double, 6, 6> hessian;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend std::ostream &operator<<(std::ostream &os, const NdtResult &val) {
+    os << "Pose: " << std::endl << val.pose << std::endl;
+    os << "TP: " << val.transform_probability << std::endl;
+    os << "NVTP: " << val.nearest_voxel_transformation_likelihood << std::endl;
+    os << "Iteration num: " << val.iteration_num << std::endl;
+    os << "Hessian: " << std::endl << val.hessian << std::endl;
+
+    return os;
+  }
 };
 
 struct NdtParams {
@@ -117,6 +127,8 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   /** \brief Typename of const pointer to searchable voxel grid leaf. */
   typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr;
 
+  typedef pcl::Registration<PointSource, PointTarget> BaseRegType;
+
 public:
 #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
   typedef pcl::shared_ptr<MultiGridNormalDistributionsTransform<PointSource, PointTarget>> Ptr;
@@ -131,6 +143,14 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    */
   MultiGridNormalDistributionsTransform();
 
+  // Copy & move constructor
+  MultiGridNormalDistributionsTransform(const MultiGridNormalDistributionsTransform &other);
+  MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform &&other);
+
+  // Copy & move assignments
+  MultiGridNormalDistributionsTransform &operator=(const MultiGridNormalDistributionsTransform &other);
+  MultiGridNormalDistributionsTransform &operator=(MultiGridNormalDistributionsTransform &&other);
+
   /** \brief Empty destructor */
   virtual ~MultiGridNormalDistributionsTransform() {}
 
@@ -142,6 +162,14 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
     return num_threads_;
   }
 
+  inline void setInputSource(const PointCloudSourceConstPtr &input) {
+    // This is to avoid segmentation fault when setting null input
+    // No idea why PCL does not check the nullity of input
+    if(input) {
+      pcl::Registration<PointSource, PointTarget>::setInputSource(input);
+    }
+  }
+
   inline void setInputTarget(const PointCloudTargetConstPtr &cloud) {
     const std::string default_target_id = "default";
     addTarget(cloud, default_target_id);
@@ -316,9 +344,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
 
 protected:
   using pcl::Registration<PointSource, PointTarget>::reg_name_;
-  using pcl::Registration<PointSource, PointTarget>::getClassName;
   using pcl::Registration<PointSource, PointTarget>::input_;
-  using pcl::Registration<PointSource, PointTarget>::indices_;
   using pcl::Registration<PointSource, PointTarget>::target_;
   using pcl::Registration<PointSource, PointTarget>::nr_iterations_;
   using pcl::Registration<PointSource, PointTarget>::max_iterations_;
@@ -327,8 +353,6 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   using pcl::Registration<PointSource, PointTarget>::transformation_;
   using pcl::Registration<PointSource, PointTarget>::transformation_epsilon_;
   using pcl::Registration<PointSource, PointTarget>::converged_;
-  using pcl::Registration<PointSource, PointTarget>::corr_dist_threshold_;
-  using pcl::Registration<PointSource, PointTarget>::inlier_threshold_;
 
   using pcl::Registration<PointSource, PointTarget>::update_visualizer_;
 

include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp

@@ -56,6 +56,101 @@
 #ifndef PCL_REGISTRATION_NDT_OMP_MULTI_VOXEL_IMPL_H_
 #define PCL_REGISTRATION_NDT_OMP_MULTI_VOXEL_IMPL_H_
 
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormalDistributionsTransform(const MultiGridNormalDistributionsTransform &other) : BaseRegType(other), target_cells_(other.target_cells_) {
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+}
+
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform &&other) : BaseRegType(std::move(other)), target_cells_(std::move(other.target_cells_)) {
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+}
+
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget> &pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::operator=(const MultiGridNormalDistributionsTransform &other) {
+  BaseRegType::operator=(other);
+
+  target_cells_ = other.target_cells_;
+
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+
+  return *this;
+}
+
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget> &pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::operator=(MultiGridNormalDistributionsTransform &&other) {
+  BaseRegType::operator=(std::move(other));
+
+  target_cells_ = std::move(other.target_cells_);
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+
+  return *this;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointSource, typename PointTarget>
 pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormalDistributionsTransform()
@@ -412,23 +507,23 @@ void pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::co
     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(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(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(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(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(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
+    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
   }
 }
 

include/pclomp/ndt_omp_impl.hpp

@@ -413,23 +413,23 @@ void pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeAngl
     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(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(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(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(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(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
+    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
   }
 }