NDT OMP

https://docs.ros.org/en/humble/p/ndt_omp/
Admin
NDT OMP is an OpenMP-boosted implementation of Normal Distributions Transform (NDT) and GICP point...
Tokens:96,364
Snippets:398
Trust Score:9.5
Update:5 months ago
Show doc for...
Tokens:
Raw
### Compute Transformation with Guess

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_ndt_omp.h.rst.txt

Computes the transformation from the input cloud to the target cloud, starting with an initial guess. This is a virtual function that must be implemented by derived classes.

```cpp
virtual void
computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess);
```

--------------------------------

### Get Neighborhood at Point (All Neighbors)

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.html

Retrieves all occupied neighboring voxels for a given reference point. This method checks all 26 surrounding cells and is slower than a radius search. It requires the point cloud to be pre-processed into voxels.

```cpp
template<typename PointT>
int
pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
    neighbors.clear();

    // Find displacement coordinates
    Eigen::Vector4i ijk(static_cast<int> (floor(reference_point.x / leaf_size_[0])),
        static_cast<int> (floor(reference_point.y / leaf_size_[1])),
        static_cast<int> (floor(reference_point.z / leaf_size_[2])), 0);
    Eigen::Array4i diff2min = min_b_ - ijk;
    Eigen::Array4i diff2max = max_b_ - ijk;
    neighbors.reserve(relative_coordinates.cols());

    // Check each neighbor to see if it is occupied and contains sufficient points
    // Slower than radius search because needs to check 26 indices
    for (int ni = 0; ni < relative_coordinates.cols(); ni++)
    {
        Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
        // Checking if the specified cell is in the grid
        if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
        {
            auto leaf_iter = leaves_.find(((ijk + displacement - min_b_).dot(divb_mul_)));
            if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_)
            {
                LeafConstPtr leaf = &(leaf_iter->second);
                neighbors.push_back(leaf);
            }
        }
    }

    return (static_cast<int> (neighbors.size()));
}
```

--------------------------------

### VoxelGridCovariance applyFilter Implementation

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.html

This snippet shows the core logic of the applyFilter method in VoxelGridCovariance. It handles input validation, bounding box calculation, voxel grid setup, and prepares for parallel processing.

```cpp
template<typename PointT>
void
pclomp::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
{
  voxel_centroids_leaf_indices_.clear ();

  // Has the input dataset been set already?
  if (!input_)
  {
    PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  // Copy the header (and thus the frame_id) + allocate enough space for points
  output.height = 1;                          // downsampling breaks the organized structure
  output.is_dense = true;                     // we filter out invalid points
  output.points.clear ();

  Eigen::Vector4f min_p, max_p;
  // Get the minimum and maximum dimensions
  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
      pcl::getMinMax3D<PointT> (input_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
  else
      pcl::getMinMax3D<PointT> (*input_, min_p, max_p);

  // Check that the leaf size is not too small, given the size of the data
  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;

  if((dx*dy*dz) > std::numeric_limits<int32_t>::max())
  {
    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
    output.clear();
    return;
  }

  // Compute the minimum and maximum bounding box values
  min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
  max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));

  // Compute the number of divisions needed along all axis
  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
  div_b_[3] = 0;

  // Clear the leaves
  leaves_.clear ();
//  leaves_.reserve(8192);

  // Set up the division multiplier
  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);

  int centroid_size = 4;

  if (downsample_all_data_)
    centroid_size = boost::mpl::size<FieldList>::value;

  // ---[ RGB special case
  std::vector<pcl::PCLPointField> fields;
  int rgba_index = -1;
  rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
  if (rgba_index == -1)
    rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
  if (rgba_index >= 0)
  {

```

--------------------------------

### Get Rotation Epsilon

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

Gets the current epsilon value for rotation convergence.

```cpp
inline double
         getRotationEpsilon () { return (rotation_epsilon_); }
```

--------------------------------

### GICP Constructor and Initialization

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

Initializes the GeneralizedIterativeClosestPoint class with default parameters and sets up the rigid transformation estimation callback.

```cpp
GeneralizedIterativeClosestPoint ()
           : k_correspondences_(20)
           , gicp_epsilon_(0.001)
           , rotation_epsilon_(2e-3)
           , mahalanobis_(0)
           , max_inner_iterations_(20)
         {
           min_number_correspondences_ = 4;
           reg_name_ = "GeneralizedIterativeClosestPoint";
           max_iterations_ = 200;
           transformation_epsilon_ = 5e-4;
           corr_dist_threshold_ = 5.;
           rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src,
                                                      const std::vector<int>& indices_src,
                                                      const PointCloudTarget& cloud_tgt,
                                                      const std::vector<int>& indices_tgt,
                                                      Eigen::Matrix4f& transformation_matrix)
           {
             estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
           };
         }
```

--------------------------------

### Get Correspondence Randomness

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

Gets the current number of correspondences used for random sampling.

```cpp
int
         getCorrespondenceRandomness () { return (k_correspondences_); }
```

--------------------------------

### Initialize GICP Transformation Computation

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp_impl.hpp.rst.txt

Sets up the necessary data structures and initial parameters for the GICP transformation computation. This includes initializing covariance matrices and setting the initial transformation guess.

```cpp
   template <typename PointSource, typename PointTarget> inline void
   pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
   {
     pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
     using namespace std;
     // Difference between consecutive transforms
     double delta = 0;
     // Get the size of the target
     const size_t N = indices_->size ();
     // Set the mahalanobis matrices to identity
     mahalanobis_.resize (N, Eigen::Matrix4f::Identity ());
     // Compute target cloud covariance matrices
     if ((!target_covariances_) || (target_covariances_->empty ()))
     {
       target_covariances_.reset (new MatricesVector);
       computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
     }
     // Compute input cloud covariance matrices
     if ((!input_covariances_) || (input_covariances_->empty ()))
     {
       input_covariances_.reset (new MatricesVector);
       computeCovariances<PointSource> (input_, tree_reciprocal_, *input_covariances_);
     }
   
     base_transformation_ = Eigen::Matrix4f::Identity();
     nr_iterations_ = 0;
     converged_ = false;
     double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
     pcl::transformPointCloud(output, output, guess);
   
     std::vector<std::vector<int>> nn_indices_array(omp_get_max_threads());
     std::vector<std::vector<float>> nn_dists_array(omp_get_max_threads());
     for (auto& nn_indices : nn_indices_array) { nn_indices.resize(1); }
     for (auto& nn_dists : nn_dists_array) { nn_dists.resize(1); }
   
     while(!converged_)
     {
       std::atomic<size_t> cnt;
       cnt = 0;
       std::vector<int> source_indices (indices_->size ());
       std::vector<int> target_indices (indices_->size ());
   
       // guess corresponds to base_t and transformation_ to t
       Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
       for(size_t i = 0; i < 4; i++)
         for(size_t j = 0; j < 4; j++)
           for(size_t k = 0; k < 4; k++)
             transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
   
       const Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
   
       #pragma omp parallel for
       for (std::size_t i = 0; i < N; i++)
       {
         auto& nn_indices = nn_indices_array[omp_get_thread_num()];
         auto& nn_dists = nn_dists_array[omp_get_thread_num()];
   
         PointSource query = output[i];
         query.getVector4fMap () = transformation_ * query.getVector4fMap ();
   
         if (!searchForNeighbors (query, nn_indices, nn_dists))
         {
           PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
           continue;
         }
   
         // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
         if (nn_dists[0] < dist_threshold)
         {
           const Eigen::Matrix3d &C1 = (*input_covariances_)[i];
           const Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
           Eigen::Matrix4f& M_ = mahalanobis_[i];
           M_.setZero();
   
           Eigen::Matrix3d M = M_.block<3, 3>(0, 0).cast<double>();
           // M = R*C1
           M = R * C1;
           // temp = M*R' + C2 = R*C1*R' + C2

```

--------------------------------

### Constructor

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/classpclomp_1_1GeneralizedIterativeClosestPoint.html

Initializes a new instance of the GeneralizedIterativeClosestPoint class.

```APIDOC
## GeneralizedIterativeClosestPoint()

### Description

Empty constructor.

### Method

`GeneralizedIterativeClosestPoint()`
```

--------------------------------

### Get Maximum Optimizer Iterations

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

Gets the maximum number of inner iterations allowed for the optimization.

```cpp
int
         getMaximumOptimizerIterations () { return (max_inner_iterations_); }
```

--------------------------------

### getCorrespondenceRandomness

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

Gets the current correspondence randomness value.

```APIDOC
## getCorrespondenceRandomness

### Description
Retrieves the current number of correspondences used for random sampling in the transformation estimation.

### Parameters
None

### Returns
The current correspondence randomness value (int).
```

--------------------------------

### GICP Transformation Estimation and Convergence Check

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp_impl.hpp.rst.txt

This snippet demonstrates the core loop of the GICP algorithm, including transformation estimation using rigid_transformation_estimation_, calculating the delta for convergence, and checking against maximum iterations or a predefined epsilon. It also handles potential PCL exceptions during optimization.

```cpp
       std::vector<std::pair<int, int>> indices(source_indices.size());
       for(std::size_t i = 0; i<source_indices.size(); i++) {
         indices[i].first = source_indices[i];
         indices[i].second = target_indices[i];
       }
   
       std::sort(indices.begin(), indices.end(), [=](const std::pair<int, int>& lhs, const std::pair<int, int>& rhs) { return lhs.first < rhs.first; });
   
       for(std::size_t i = 0; i < source_indices.size(); i++) {
         source_indices[i] = indices[i].first;
         target_indices[i] = indices[i].second;
       }
   
       /* optimize transformation using the current assignment and Mahalanobis metrics*/
       previous_transformation_ = transformation_;
       //optimization right here
       try
       {
         rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
         /* compute the delta from this iteration */
         delta = 0.;
         for(int k = 0; k < 4; k++) {
           for(int l = 0; l < 4; l++) {
             double ratio = 1;
             if(k < 3 && l < 3) // rotation part of the transform
               ratio = 1./rotation_epsilon_;
             else
               ratio = 1./transformation_epsilon_;
             double c_delta = ratio*std::abs(previous_transformation_(k,l) - transformation_(k,l));
             if(c_delta > delta)
               delta = c_delta;
           }
         }
       }
       catch (pcl::PCLException &e)
       {
         PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
         break;
       }
       nr_iterations_++;
       // Check for convergence
       if (nr_iterations_ >= max_iterations_ || delta < 1)
       {
         converged_ = true;
         previous_transformation_ = transformation_;
         PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
                    getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
       }
       else
         PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
     }
     final_transformation_ = previous_transformation_ * guess;
   
     // Transform the point cloud
     pcl::transformPointCloud (*input_, output, final_transformation_);
   
```

--------------------------------

### getCorrespondenceRandomness

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_gicp_omp.h.html

Gets the number of correspondences used for random sampling.

```APIDOC
## int getCorrespondenceRandomness()

### Description
Retrieves the current number of correspondences used for random sampling in the GICP algorithm.

### Returns
- (int): The number of correspondences for random sampling.
```

--------------------------------

### Benchmark ndt_omp Algorithms

Source: https://docs.ros.org/en/humble/p/ndt_omp/index.html

Run this command to benchmark the performance of different NDT algorithms (PCL, pclomp with KDTREE, DIRECT7, DIRECT1) using single and multi-threading. It compares execution times and fitness scores for two PCD files.

```bash
$ roscd ndt_omp/data
$ rosrun ndt_omp align 251370668.pcd 251371071.pcd
```

--------------------------------

### Get Covariance Eigenvalue Inflation Ratio

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.rst.txt

Retrieves the current multiplier for inflating covariance matrix eigenvalues.

```cpp
inline double
        getCovEigValueInflationRatio ()
        {
          return min_covar_eigvalue_mult_;
        }
```

--------------------------------

### Get Mahalanobis Matrix

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

Retrieves the Mahalanobis matrix for a given index. Asserts that the index is within bounds.

```cpp
inline const Eigen::Matrix4f& mahalanobis(size_t index) const
         {
           assert(index < mahalanobis_.size());
           return mahalanobis_[index];
         }
```

--------------------------------

### Constructor

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/classpclomp_1_1VoxelGridCovariance.html

Initializes a new instance of the VoxelGridCovariance class. Sets the leaf size to 0 and searchable to false by default.

```APIDOC
## VoxelGridCovariance()

### Description
Constructor. Sets leaf_size_ to 0 and searchable_ to false.

### Method
`VoxelGridCovariance()`
```

--------------------------------

### Get Voxel Centroids

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.html

Returns a shared pointer to the point cloud containing the centroids of all voxels. This cloud is generated after filtering.

```cpp
inline PointCloudPtr
      getCentroids ()
      {
        return voxel_centroids_;
      }
```

--------------------------------

### Benchmark ndt_omp Alignment

Source: https://docs.ros.org/en/humble/p/ndt_omp/__README.html

Run the alignment benchmark using the 'align' executable with two PCD files. This demonstrates the performance differences between pcl::NDT and pclomp::NDT with various configurations and thread counts.

```bash
$ roscd ndt_omp/data
$ rosrun ndt_omp align 251370668.pcd 251371071.pcd
--- pcl::NDT ---
single : 282.222[msec]
10times: 2921.92[msec]
fitness: 0.213937

--- pclomp::NDT (KDTREE, 1 threads) ---
single : 207.697[msec]
10times: 2059.19[msec]
fitness: 0.213937

--- pclomp::NDT (DIRECT7, 1 threads) ---
single : 139.433[msec]
10times: 1356.79[msec]
fitness: 0.214205

--- pclomp::NDT (DIRECT1, 1 threads) ---
single : 34.6418[msec]
10times: 317.03[msec]
fitness: 0.208511

--- pclomp::NDT (KDTREE, 8 threads) ---
single : 54.9903[msec]
10times: 500.51[msec]
fitness: 0.213937

--- pclomp::NDT (DIRECT7, 8 threads) ---
single : 63.1442[msec]
10times: 343.336[msec]
fitness: 0.214205

--- pclomp::NDT (DIRECT1, 8 threads) ---
single : 17.2353[msec]
10times: 100.025[msec]
fitness: 0.208511

```

--------------------------------

### Generate Display Cloud

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.rst.txt

Generates a point cloud for visualization purposes, using normal distributions and Cholesky decomposition for covariance matrices.

```cpp
   
   
   template<typename PointT>
   void
   pclomp::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud)
   {
     cell_cloud.clear ();
   
     int pnt_per_cell = 1000;
     boost::mt19937 rng;
     boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
   
     Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
     Eigen::Matrix3d cholesky_decomp;

```

--------------------------------

### Get All Leaves

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.html

Returns a constant reference to the map containing all leaf structures. This map stores the computed data for each voxel.

```cpp
inline const Map&
      getLeaves ()
      {
        return leaves_;
      }
```

--------------------------------

### Initialize NormalDistributionsTransform Constructor

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_ndt_omp_impl.hpp.rst.txt

Initializes the NDT parameters, including resolution, step size, and outlier ratio. It also pre-calculates Gaussian fitting parameters based on these settings. This constructor sets up the NDT object for use.

```cpp
#include "ndt_omp.h"
/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_
#define PCL_REGISTRATION_NDT_OMP_IMPL_H_

template<typename PointSource, typename PointTarget>
pclomp::NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform ()
  : target_cells_ ()
  , resolution_ (1.0f)
  , step_size_ (0.1)
  , outlier_ratio_ (0.55)
  , gauss_d1_ ()
  , gauss_d2_ ()
  , gauss_d3_ ()
  , trans_probability_ ()
  , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ ()
  , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ ()
  , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ ()
{
  reg_name_ = "NormalDistributionsTransform";

  double gauss_c1, gauss_c2;

  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
  gauss_c1 = 10.0 * (1 - outlier_ratio_);
  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
  gauss_d3_ = -log (gauss_c2);
  gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_;
  gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_);

  transformation_epsilon_ = 0.1;
  max_iterations_ = 35;

  search_method = DIRECT7;
  num_threads_ = omp_get_max_threads();
}

```

--------------------------------

### Get Neighborhood at Point (1 Neighbor)

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.rst.txt

Retrieves only the immediate voxel containing the reference point. This is used for single-cell analysis.

```cpp
   
   
   template<typename PointT>
   int
   pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
   {
       neighbors.clear();
       return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors);
   }

```

--------------------------------

### init

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/classpclomp_1_1NormalDistributionsTransform.html

Initializes the covariance voxel structure.

```APIDOC
## init

### Description
Initiate covariance voxel structure.

### Method Signature
`inline void init()`
```

--------------------------------

### Get Minimum Points Per Voxel

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.rst.txt

Retrieves the current minimum number of points required per voxel for covariance calculation.

```cpp
inline int
        getMinPointPerVoxel ()
        {
          return min_points_per_voxel_;
        }
```

--------------------------------

### Get Leaf by Index

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.rst.txt

Retrieves a constant pointer to a Leaf structure based on its index. Returns NULL if the index is not found in the internal map.

```cpp
inline LeafConstPtr
        getLeaf (int index)
        {
          auto leaf_iter = leaves_.find (index);
          if (leaf_iter != leaves_.end ()) 
          {
            LeafConstPtr ret (&(leaf_iter->second));
            return ret;
          }
          else
            return NULL;
        }
```

--------------------------------

### getDisplayCloud

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.html

Generates a point cloud for visualization purposes, sampling points from each occupied voxel based on its mean and covariance. This allows for visual inspection of the voxel grid's distribution.

```APIDOC
## getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud)

### Description
Generates a point cloud for visualization purposes, sampling points from each occupied voxel based on its mean and covariance. This allows for visual inspection of the voxel grid's distribution.

### Parameters
#### Path Parameters
- `cell_cloud` (pcl::PointCloud<pcl::PointXYZ>&) - An output point cloud to which the generated points will be added.
```

--------------------------------

### Get Neighborhood at Point (7 Neighbors)

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.rst.txt

Retrieves a specific set of 7 neighboring voxels for a given point. This is a specialized version of the neighborhood search.

```cpp
   template<typename PointT>
   int
   pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
   {
       neighbors.clear();
   
       Eigen::MatrixXi relative_coordinates(3, 7);
       relative_coordinates.setZero();
       relative_coordinates(0, 1) = 1;
       relative_coordinates(0, 2) = -1;
       relative_coordinates(1, 3) = 1;
       relative_coordinates(1, 4) = -1;
       relative_coordinates(2, 5) = 1;
       relative_coordinates(2, 6) = -1;
   
       return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
   }

```

--------------------------------

### More-Thuente Step Length Calculation (NDT)

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_ndt_omp_impl.hpp.rst.txt

Implements the More-Thuente line search algorithm to find an optimal step length for NDT optimization. It ensures sufficient decrease and satisfies curvature conditions.

```cpp
   template<typename PointSource, typename PointTarget>
   double
   pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
                                                                                     double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
                                                                                     PointCloudSource &trans_cloud)
   {
     // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
     double phi_0 = -score;
     // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
     double d_phi_0 = -(score_gradient.dot (step_dir));
   
     Eigen::Matrix<double, 6, 1>  x_t;
   
     if (d_phi_0 >= 0)
     {
       // Not a decent direction
       if (d_phi_0 == 0)
         return 0;
       else
       {
         // Reverse step direction and calculate optimal step.
         d_phi_0 *= -1;
         step_dir *= -1;
   
       }
     }
   
     // The Search Algorithm for T(mu) [More, Thuente 1994]
   
     int max_step_iterations = 10;
     int step_iterations = 0;
   
     // Sufficient decrease constant, Equation 1.1 [More, Thuete 1994]
     double mu = 1.e-4;
     // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
     double nu = 0.9;
   
     // Initial endpoints of Interval I,
     double a_l = 0, a_u = 0;
   
     // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
     double f_l = auxiliaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
     double g_l = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
   
     double f_u = auxiliaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
     double g_u = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
   
     // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
     bool interval_converged = (step_max - step_min) < 0, open_interval = true;
   
     double a_t = step_init;
     a_t = std::min (a_t, step_max);
     a_t = std::max (a_t, step_min);
   
     x_t = x + step_dir * a_t;
   
     final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) * 
                              Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) * 
                              Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) * 
                              Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
   
     // New transformed point cloud
     transformPointCloud (*input_, trans_cloud, final_transformation_);
   
     // Updates score, gradient and hessian.  Hessian calculation is unnecessary but testing showed that most step calculations use the
     // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
     score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);
   
     // Calculate phi(alpha_t)
     double phi_t = -score;
     // Calculate phi'(alpha_t)
     double d_phi_t = -(score_gradient.dot (step_dir));
   
     // Calculate psi(alpha_t)
     double psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
     // Calculate psi'(alpha_t)
     double d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
   
     // Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
     while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
     {
       // Use auxiliary function if interval I is not closed
       if (open_interval)
       {
         a_t = trialValueSelectionMT (a_l, f_l, g_l,
                                      a_u, f_u, g_u,
                                      a_t, psi_t, d_psi_t);
       }
       else
       {
         a_t = trialValueSelectionMT (a_l, f_l, g_l,
                                      a_u, f_u, g_u,
                                      a_t, phi_t, d_phi_t);
       }
   
       a_t = std::min (a_t, step_max);
       a_t = std::max (a_t, step_min);
   
       x_t = x + step_dir * a_t;
   
```

--------------------------------

### getDisplayCloud

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.rst.txt

Populates a point cloud with data for display, representing voxel cells.

```APIDOC
## getDisplayCloud

### Description
Populates a point cloud with data for display, representing voxel cells.

### Method Signature
```cpp
void getDisplayCloud(pcl::PointCloud<pcl::PointXYZ>& cell_cloud)
```

### Parameters
* **cell_cloud** (pcl::PointCloud<pcl::PointXYZ>&) - The output point cloud to be populated with voxel cell information.
```

--------------------------------

### Get Neighborhood at Point (1 Neighbor)

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.html

Retrieves only the voxel at the reference point's location (0,0,0 displacement). This is a specialized case for checking the current voxel.

```cpp
template<typename PointT>
int
pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
    neighbors.clear();
    return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors);
}
```

--------------------------------

### GeneralizedIterativeClosestPoint Constructor

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

Initializes the GeneralizedIterativeClosestPoint object with default parameters.

```APIDOC
## GeneralizedIterativeClosestPoint()

### Description
Initializes the GeneralizedIterativeClosestPoint object with default parameters for GICP.

### Parameters
None

### Returns
A new instance of GeneralizedIterativeClosestPoint.
```

--------------------------------

### Set Input Source Point Cloud

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_gicp_omp.h.html

Sets the source point cloud for the GICP algorithm. It checks for empty clouds and prepares the input by setting the homogeneous coordinate to 1.0.

```cpp
inline void
      setInputSource (const PointCloudSourceConstPtr &cloud) override
      {

        if (cloud->points.empty ()) 
        {
          PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
          return;
        }
        PointCloudSource input = *cloud;
        // Set all the point.data[3] values to 1 to aid the rigid transformation
        for (size_t i = 0; i < input.size (); ++i)
          input[i].data[3] = 1.0;

        pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputSource (cloud);
        input_covariances_.reset ();
      }
```

--------------------------------

### Get Neighborhood at Point (7 Neighbors)

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.html

Retrieves neighboring voxels using a specific set of 7 relative coordinates. This can be useful for scenarios where only a subset of neighbors is relevant.

```cpp
template<typename PointT>
int
pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
    neighbors.clear();

    Eigen::MatrixXi relative_coordinates(3, 7);
    relative_coordinates.setZero();
    relative_coordinates(0, 1) = 1;
    relative_coordinates(0, 2) = -1;
    relative_coordinates(1, 3) = 1;
    relative_coordinates(1, 4) = -1;
    relative_coordinates(2, 5) = 1;
    relative_coordinates(2, 6) = -1;

    return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
}
```

--------------------------------

### Get Neighborhood at Point (Default Neighbors)

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp_impl.hpp.html

Retrieves neighboring voxels using the default set of relative coordinates (all 26 neighbors). This is a convenience function that calls the more general `getNeighborhoodAtPoint`.

```cpp
template<typename PointT>
int
pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
    neighbors.clear();

    // Find displacement coordinates
    Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices();
    return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
}
```

--------------------------------

### GICP OMP Header File

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_gicp_omp.h.rst.txt

This snippet includes the license, header guards, and necessary includes for the GeneralizedIterativeClosestPoint class, which is an OpenMP-enabled ICP implementation.

```cpp
/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */
   
#ifndef PCL_GICP_OMP_H_
#define PCL_GICP_OMP_H_
   
#include <pcl/registration/icp.h>
#include <pcl/registration/bfgs.h>
   
namespace pclomp
{
  template <typename PointSource, typename PointTarget>
  class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<PointSource, PointTarget>
  {
    public:
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::getClassName;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::indices_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::target_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::input_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::tree_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::tree_reciprocal_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::transformation_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::converged_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
      using pcl::IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_;

      using PointCloudSource = pcl::PointCloud<PointSource>;
      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

      using PointCloudTarget = pcl::PointCloud<PointTarget>;
      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
      using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

      using PointIndicesPtr = pcl::PointIndices::Ptr;
      using PointIndicesConstPtr = pcl::PointIndices::ConstPtr;

      using InputKdTree = typename pcl::Registration<PointSource, PointTarget>::KdTree;
      using InputKdTreePtr = typename pcl::Registration<PointSource, PointTarget>::KdTreePtr;

```

--------------------------------

### Get Leaf by Point (PointT)

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.html

Retrieves a constant pointer to the Leaf structure containing the given PointT. Calculates the voxel index based on the point's coordinates and the leaf size.

```cpp
inline LeafConstPtr
      getLeaf (PointT &p)
      {
        // Generate index associated with p
        int ijk0 = static_cast<int> (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
        int ijk1 = static_cast<int> (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
        int ijk2 = static_cast<int> (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);

        // Compute the centroid leaf index
        int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];

        // Find leaf associated with index
        auto leaf_iter = leaves_.find (idx);
        if (leaf_iter != leaves_.end ()) 
        {
          // If such a leaf exists return the pointer to the leaf structure
          LeafConstPtr ret (&(leaf_iter->second));
          return ret;
        }
        else
          return NULL;
      }
```

--------------------------------

### filter (PointCloud output, bool searchable)

Source: https://docs.ros.org/en/humble/p/ndt_omp/generated/classpclomp_1_1VoxelGridCovariance.html

Filters the input point cloud and initializes the voxel structure. Optionally builds a k-d tree if searchable is true.

```APIDOC
## filter(PointCloud &output, bool searchable = false)

### Description
Filter cloud and initializes voxel structure.

### Method
`void filter(PointCloud &output, bool searchable = false)`

### Parameters
#### Path Parameters
- **output** (PointCloud&) - Required - Cloud containing centroids of voxels containing a sufficient number of points
- **searchable** (bool) - Optional - Flag if voxel structure is searchable, if true then kdtree is built
```

--------------------------------

### VoxelGridCovariance Constructor

Source: https://docs.ros.org/en/humble/p/ndt_omp/_sources/generated/program_listing_file_include_pclomp_voxel_grid_covariance_omp.h.rst.txt

Initializes a new instance of the VoxelGridCovariance class with default parameters.

```APIDOC
## VoxelGridCovariance()

### Description
Initializes a new instance of the VoxelGridCovariance class with default parameters.

### Method
Constructor

### Parameters
None
```
Tokens:
Raw