### Setup PPA for GTSAM and gtsam_points

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Downloads and executes a script to set up the PPA repository for installing GTSAM and gtsam_points on Ubuntu.

```bash
curl -s https://koide3.github.io/ppa/setup_ppa.sh | sudo bash
```

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

### Install GTSAM from Source

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Clones the GTSAM repository, checks out a specific version, configures the build with specific options (disabling examples, tests, TBB, and march-native), and installs it.

```bash
# Install gtsam
git clone https://github.com/borglab/gtsam
cd gtsam
git checkout 4.3a1

mkdir build && cd build
cmake .. \
  -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
  -DGTSAM_BUILD_TESTS=OFF \
  -DGTSAM_WITH_TBB=OFF \
  -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF

make -j$(nproc)
sudo make install
```

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

### Install Iridescence Visualization Library

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Installs the iridescence library, which is optional and required only for demo programs. It installs dependencies and then builds and installs the library from source.

```bash
# [optional] Install iridescence visualization library
# This is required for only demo programs
sudo apt install -y libglm-dev libglfw3-dev libpng-dev
git clone https://github.com/koide3/iridescence --recursive
mkdir iridescence/build && cd iridescence/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install
```

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

### GNC Registration Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-registration.md

Example demonstrating how to set up GNC parameters and perform registration. Ensure you have initialized point clouds, features, and nearest neighbor search structures.

```cpp
#include <gtsam_points/registration/graduated_non_convexity.hpp>

// Setup GNC parameters
gtsam_points::GNCParams gnc_params;
gnc_params.max_iterations = 64;
gnc_params.dof = 6;
gnc_params.verbose = true;
gnc_params.num_threads = 4;

// Run GNC registration
auto result = gtsam_points::estimate_pose_gnc(
  *target_cloud,
  *source_cloud,
  target_features.data(),
  source_features.data(),
  *target_tree,
  *target_features_tree,
  *source_features_tree,
  gnc_params
);

if (result.converged) {
  std::cout << "Converged in " << result.num_iterations << " iterations" << std::endl;
  std::cout << "Inlier fraction: " << (100.0 * result.inlier_fraction) << "%" << std::endl;
}

```

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

### Usage Example: IntegratedVGICPFactorGPU vs CPU Fallback

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-specialized-factors.md

This example demonstrates how to conditionally use the IntegratedVGICPFactorGPU when CUDA is enabled, falling back to the CPU version otherwise. It shows the setup for the GPU voxel map and factor creation.

```cpp
#ifdef BUILD_WITH_CUDA
// Use GPU factor
auto gpu_voxelmap = std::make_shared<GaussianVoxelMapGPU>(0.1);
gpu_voxelmap->insert(*source_gpu);

auto gpu_factor = std::make_shared<IntegratedVGICPFactorGPU>(
  x0, x1, gpu_voxelmap, source_gpu
);
graph.add(gpu_factor);
#else
// Fall back to CPU
auto cpu_factor = std::make_shared<IntegratedVGICPFactor>(
  x0, x1, cpu_voxelmap, source_cpu
);
graph.add(cpu_factor);
#endif

```

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

### Install gtsam_points via PPA (With CUDA 13.1)

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Installs the CUDA 13.1 enabled version of the gtsam_points development package from the PPA.

```bash
# with CUDA 13.1
sudo apt install -y libgtsam-points-cuda13.1-dev
```

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

### ISAM2Ext Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Demonstrates the initialization and usage of the ISAM2Ext solver, including setting parameters and performing updates.

```cpp
#include <gtsam_points/optimizers/isam2_ext.hpp>

gtsam_points::ISAM2ExtParams params;
params.relinearizeThreshold = 0.01;
params.relinearizeSkip = 1;

auto isam2 = std::make_shared<gtsam_points::ISAM2Ext>(params);

// First update
gtsam::NonlinearFactorGraph factors1;
gtsam::Values values1;
// Add factors and values...
auto result1 = isam2->update(factors1, values1);

// Subsequent updates (incremental)
gtsam::NonlinearFactorGraph factors2;
gtsam::Values values2;
// Add new factors and values...
auto result2 = isam2->update(factors2, values2);

// Get final estimates
const auto& estimates = isam2->calculateEstimate();
```

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

### KdTree Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Demonstrates creating a KdTree and performing both k-nearest neighbors and radius searches.

```cpp
// Create KdTree from point cloud
std::vector<Eigen::Vector4d> points = ...;
auto kdtree = std::make_shared<KdTree>(
  points.data(),
  points.size(),
  4  // 4 threads for building
);

// KNN search
size_t k = 5;
std::vector<size_t> k_indices(k);
std::vector<double> k_sq_dists(k);
size_t found = kdtree->knn_search(
  &query_point[0],
  k,
  k_indices.data(),
  k_sq_dists.data()
);

// Radius search
std::vector<size_t> radius_indices;
std::vector<double> radius_dists;
kdtree->radius_search(
  &query_point[0],
  1.0,  // 1 meter radius
  radius_indices,
  radius_dists
);

```

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

### Stopwatch Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Demonstrates how to use the Stopwatch utility for measuring the execution time of a specific computation. Start the timer before the operation and stop it afterward to retrieve the elapsed time in seconds.

```cpp
gtsam_points::Stopwatch timer;
timer.start();

// Some computation
perform_registration();

timer.stop();
std::cout << "Time: " << timer.elapsed_secs() << " seconds" << std::endl;
```

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

### Install gtsam_points via PPA (With CUDA 12.5)

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Installs the CUDA 12.5 enabled version of the gtsam_points development package from the PPA.

```bash
# with CUDA 12.5
sudo apt install -y libgtsam-points-cuda12.5-dev
```

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

### Install gtsam_points via PPA (Without CUDA)

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Installs the gtsam_points development package from the PPA without CUDA support.

```bash
# Without CUDA
sudo apt install -y libgtsam-points-dev
```

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

### IntegratedCT_ICPFactor Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-specialized-factors.md

Example demonstrating how to create and configure an IntegratedCT_ICPFactor. Ensure the target tree is properly initialized and add the factor to the graph.

```cpp
auto ct_icp = std::make_shared<IntegratedCT_ICPFactor>(
  x0, x1,
  target_cloud,
  source_cloud,
  target_tree
);

auto* factor = static_cast<IntegratedCT_ICPFactor*>(ct_icp.get());
factor->set_max_correspondence_distance(1.0);
graph.add(ct_icp);
```

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

### Configure and use IntegratedGICPFactor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Example demonstrating the initialization of an IntegratedGICPFactor and subsequent configuration of its caching mode, correspondence distance, and thread count.

```cpp
auto gicp_factor = std::make_shared<IntegratedGICPFactor>(
  gtsam::Symbol('x', 0),
  gtsam::Symbol('x', 1),
  target_cloud,
  source_cloud,
  target_kdtree
);

auto* factor_ptr = static_cast<IntegratedGICPFactor*>(gicp_factor.get());
factor_ptr->set_fused_cov_cache_mode(FusedCovCacheMode::COMPACT);
factor_ptr->set_max_correspondence_distance(1.5);
factor_ptr->set_num_threads(4);
```

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

### PointCloud Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-types.md

Demonstrates how to create a PointCloud instance, manually allocate and assign point and normal data, check for data availability, and save the cloud to a file.

```cpp
// Create a point cloud with positions and normals
auto cloud = std::make_shared<gtsam_points::PointCloud>();
cloud->num_points = 1000;
cloud->points = new Eigen::Vector4d[1000];
cloud->normals = new Eigen::Vector4d[1000];

// Fill with data...
for (size_t i = 0; i < 1000; ++i) {
  cloud->points[i] = Eigen::Vector4d(x, y, z, 1.0);
  cloud->normals[i] = Eigen::Vector4d(nx, ny, nz, 0.0);
}

// Check availability
if (cloud->check_points() && cloud->check_normals()) {
  // Use in algorithms...
}

// Save
cloud->save("output.pcd");
```

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

### Initialize IntegratedPointToPlaneICPFactor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Example of creating and initializing an IntegratedPointToPlaneICPFactor with target and source point clouds and a target kdtree.

```cpp
auto point_to_plane_factor = std::make_shared<IntegratedPointToPlaneICPFactor>(
  gtsam::Symbol('x', 0),
  gtsam::Symbol('x', 1),
  target_cloud,
  source_cloud,
  target_kdtree
);
```

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

### ContinuousTrajectory Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Demonstrates the creation of a ContinuousTrajectory, fitting it to measurements, and querying interpolated poses. This example requires including the necessary header and providing measurement data.

```cpp
#include <gtsam_points/util/continuous_trajectory.hpp>

// Create a trajectory from t=0 to t=10 seconds with 0.1s knot spacing
gtsam_points::ContinuousTrajectory traj('x', 0.0, 10.0, 0.1);

// Fit spline to measurements
std::vector<double> timestamps = {0.0, 1.0, 2.0, 3.0};
std::vector<gtsam::Pose3> measured_poses = {...};

auto knots = traj.fit_knots(
  timestamps,
  measured_poses,
  1e-3,  // Smoothness regularization
  true   // Verbose
);

// Query interpolated pose at arbitrary time
gtsam::Pose3 pose_at_1_5s = traj.pose(knots, 1.5);

// Use in optimization (expression-based)
// auto error = pose_at_1_5s - measured_pose;
```

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

### LevenbergMarquardtOptimizerExt Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Demonstrates how to create and use the LevenbergMarquardtOptimizerExt. Includes initialization with default and custom parameters, and running the optimization.

```cpp
#include <gtsam_points/optimizers/levenberg_marquardt_ext.hpp>

// Create factor graph and initial values
gtsam::NonlinearFactorGraph graph;
gtsam::Values initialValues;

// Add factors and values...
// graph.add(...);
// initialValues.insert(...);

// Create optimizer with default parameters
auto optimizer = std::make_shared<gtsam_points::LevenbergMarquardtOptimizerExt>(
  graph,
  initialValues
);

// Or with custom parameters
gtsam_points::LevenbergMarquardtExtParams params;
params.maxIterations = 100;
params.relativeErrorTol = 1e-4;

auto custom_optimizer = std::make_shared<gtsam_points::LevenbergMarquardtOptimizerExt>(
  graph,
  initialValues,
  params
);

// Optimize
const auto& result = custom_optimizer->optimize();
std::cout << "Final lambda: " << custom_optimizer->lambda() << std::endl;
```

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

### Region Growing Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-features-segmentation.md

Demonstrates how to set up and use the region growing algorithm to find clusters in a point cloud. Includes initialization, iterative updating, and dilation.

```cpp
#include <gtsam_points/segmentation/region_growing.hpp>

// Setup point cloud and search structure
auto kdtree = std::make_shared<gtsam_points::KdTree>(
  points.data(), points.size(), 4
);

// Configure region growing
gtsam_points::RegionGrowingParams rg_params;
rg_params.distance_threshold = 0.2;
rg_params.angle_threshold = M_PI / 6.0;  // 30 degrees
rg_params.dilation_radius = 0.1;

// Find clusters
std::vector<std::vector<size_t>> clusters;

for (size_t i = 0; i < points.size(); ++i) {
  // Skip if already in a cluster
  bool already_clustered = false;
  for (const auto& cluster : clusters) {
    if (std::find(cluster.begin(), cluster.end(), i) != cluster.end()) {
      already_clustered = true;
      break;
    }
  }
  if (already_clustered) continue;
  
  // Initialize region growing
  auto context = gtsam_points::region_growing_init(
    *cloud,
    *kdtree,
    points[i],
    rg_params
  );
  
  // Run until convergence
  gtsam_points::region_growing_update(context, *cloud, *kdtree, rg_params);
  
  // Dilate
  gtsam_points::region_growing_dilation_(context, *cloud, *kdtree, rg_params);
  
  clusters.push_back(context.cluster_indices);
}

std::cout << "Found " << clusters.size() << " clusters" << std::endl;
```

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

### IntegratedVGICPFactor Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Demonstrates initializing a GaussianVoxelMapCPU, inserting a point cloud, and creating an IntegratedVGICPFactor. It also shows how to set the number of threads and covariance cache mode.

```cpp
// Build Gaussian voxelmap
auto voxelmap = std::make_shared<GaussianVoxelMapCPU>(0.1);  // 0.1m resolution
voxelmap->insert(*target_cloud);

auto vgicp_factor = std::make_shared<IntegratedVGICPFactor>(
  gtsam::Symbol('x', 0),
  gtsam::Symbol('x', 1),
  voxelmap,
  source_cloud
);

auto* factor_ptr = static_cast<IntegratedVGICPFactor*>(vgicp_factor.get());
factor_ptr->set_num_threads(4);
factor_ptr->set_fused_cov_cache_mode(FusedCovCacheMode::FULL);

```

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

### Install gtsam_points via PPA (With CUDA 12.2)

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Installs the CUDA 12.2 enabled version of the gtsam_points development package from the PPA. This option is specific to Ubuntu 22.04.

```bash
# with CUDA 12.2 (for only Ubuntu 22.04)
sudo apt install -y libgtsam-points-cuda12.2-dev
```

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

### IntegratedVGICPFactor Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Demonstrates how to instantiate and configure an IntegratedVGICPFactor, including building a Gaussian voxelmap and setting parameters like the number of threads and cache mode.

```APIDOC
## Usage Example

```cpp
// Build Gaussian voxelmap
auto voxelmap = std::make_shared<GaussianVoxelMapCPU>(0.1);  // 0.1m resolution
voxelmap->insert(*target_cloud);

auto vgicp_factor = std::make_shared<IntegratedVGICPFactor>(
  gtsam::Symbol('x', 0),
  gtsam::Symbol('x', 1),
  voxelmap,
  source_cloud
);

auto* factor_ptr = static_cast<IntegratedVGICPFactor*>(vgicp_factor.get());
factor_ptr->set_num_threads(4);
factor_ptr->set_fused_cov_cache_mode(FusedCovCacheMode::FULL);
```
```

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

### GaussianVoxelMapCPU Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-types.md

Demonstrates the typical workflow for using GaussianVoxelMapCPU, including initialization, configuration of LRU deletion, incremental insertion of point clouds, and performing nearest neighbor searches.

```cpp
// Create voxelmap with 10cm resolution
auto voxelmap = std::make_shared<gtsam_points::GaussianVoxelMapCPU>(0.1);

// Configure LRU deletion
voxelmap->set_lru_horizon(2000);
voxelmap->set_lru_clear_cycle(100);
voxelmap->set_neighbor_voxel_mode(27);  // 27-neighbor search

// Insert point clouds incrementally
for (const auto& cloud : clouds) {
  voxelmap->insert(*cloud);
}

// Look up a voxel
Eigen::Vector3i coord = Eigen::Vector3i(10, 20, 30);
int voxel_id = voxelmap->lookup_voxel_index(coord);
if (voxel_id >= 0) {
  const auto& voxel = voxelmap->lookup_voxel(voxel_id);
  std::cout << "Voxel mean: " << voxel.mean.transpose() << std::endl;
}

// Nearest neighbor search
std::vector<size_t> indices(5);
std::vector<double> dists(5);
voxelmap->knn_search(&query[0], 5, indices.data(), dists.data());
```

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

### FastOccupancyGrid Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Demonstrates the creation and usage of FastOccupancyGrid for computing overlap between two point clouds. It shows how to initialize grids and use the overlap method with a pose estimate.

```cpp
auto grid1 = std::make_shared<FastOccupancyGrid>(
  points1.data(), points1.size(), 0.1  // 0.1m resolution
);
auto grid2 = std::make_shared<FastOccupancyGrid>(
  points2.data(), points2.size(), 0.1
);

double overlap_ratio = grid1->overlap(*grid2, pose_estimate);
```

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

### IncrementalVoxelMap Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Illustrates the typical workflow for using IncrementalVoxelMap, including creation, configuration, dynamic point insertion, nearest neighbor queries, and reconstructing point data.

```cpp
// Create incremental voxelmap
auto voxelmap = std::make_shared<IncrementalVoxelMap<MyVoxelType>>(0.1);

// Configure
voxelmap->set_neighbor_voxel_mode(27);      // Full 27-neighbor search
voxelmap->set_lru_horizon(1000);            // Delete after 1000 steps
voxelmap->set_lru_clear_cycle(100);         // Check every 100 insertions

// Insert points dynamically
for (auto& cloud : dynamic_point_clouds) {
  voxelmap->insert(cloud);
}

// Query
size_t k = 5;
std::vector<size_t> indices(k);
std::vector<double> dists(k);
voxelmap->knn_search(&query[0], k, indices.data(), dists.data());

// Reconstruct point locations
for (size_t idx : indices) {
  auto point = voxelmap->point(idx);
  size_t v_id = voxelmap.voxel_id(idx);
  size_t p_id = voxelmap.point_id(idx);
}
```

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

### Build gtsam_points from Source

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Clones the gtsam_points repository, configures the build with Release type, and installs it. Includes commented-out optional CMake arguments for further customization.

```bash
## Build gtsam_points
git clone https://github.com/koide3/gtsam_points
mkdir gtsam_points/build && cd gtsam_points/build
cmake .. -DCMAKE_BUILD_TYPE=Release

# Optional cmake arguments
# cmake .. \
#   -DBUILD_DEMO=OFF \
#   -DBUILD_TESTS=OFF \
#   -DBUILD_TOOLS=OFF \
#   -DBUILD_WITH_TBB=OFF \
#   -DBUILD_WITH_OPENMP=OFF \
#   -DBUILD_WITH_CUDA=OFF \
#   -DBUILD_WITH_CUDA_MULTIARCH=OFF \
#   -DCMAKE_CUDA_ARCHITECTURES=89 \
#   -DBUILD_WITH_MARCH_NATIVE=OFF

make -j$(nproc)
sudo make install
```

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

### Usage Example for IntegratedLOAMFactor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Demonstrates the creation of an IntegratedLOAMFactor using shared pointers to point clouds and a k-d tree, followed by setting the maximum correspondence distance. This shows a typical initialization and configuration pattern.

```cpp
auto loam_factor = std::make_shared<IntegratedLOAMFactor>(
  gtsam::Symbol('x', 0),
  gtsam::Symbol('x', 1),
  target_cloud,
  source_cloud,
  target_kdtree
);

auto* factor_ptr = static_cast<IntegratedLOAMFactor*>(loam_factor.get());
factor_ptr->set_max_correspondence_distance(1.0);

```

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

### Usage Example: Binary ICP Factor Configuration

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Demonstrates how to create and configure a binary IntegratedICPFactor. This includes setting the maximum correspondence distance, number of threads, using point-to-plane distance, and defining update tolerances.

```cpp
// Create a binary ICP factor
auto icp_factor = std::make_shared<gtsam::NonlinearFactor::shared_ptr>(
  new IntegratedICPFactor(
    gtsam::Symbol('x', 0),           // target pose key
    gtsam::Symbol('x', 1),           // source pose key
    target_cloud,
    source_cloud,
    target_kdtree
  )
);

// Configure factor behavior
IntegratedICPFactor* factor = static_cast<IntegratedICPFactor*>(icp_factor.get());
factor->set_max_correspondence_distance(1.0);  // 1 meter cutoff
factor->set_num_threads(4);
factor->set_point_to_plane_distance(true);     // Use point-to-plane
factor->set_correspondence_update_tolerance(0.1, 0.05);  // 0.1 rad, 0.05 m
```

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

### Get Optimizer Parameters

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Provides read-only access to the current optimizer parameters.

```cpp
const LevenbergMarquardtExtParams& params() const;
```

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

### CustomLinearizationHook Implementation

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Example implementation of the LinearizationHook interface for custom linearization logic. This can be passed to an optimizer via its parameters.

```cpp
class CustomLinearizationHook : public gtsam_points::LinearizationHook {
public:
  void hook(gtsam::GaussianFactorGraph& factors) override {
    // Process GPU factors, modify Gaussian factors, etc.
  }
};

// Pass to optimizer via params
```

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

### IntegratedColoredGICPFactor Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-specialized-factors.md

Creates and configures an IntegratedColoredGICPFactor, setting the fused covariance cache mode and number of threads before adding it to the graph.

```cpp
auto colored_gicp = std::make_shared<IntegratedColoredGICPFactor>(
  x0, x1,
  target_cloud,
  source_cloud,
  target_tree
);

auto* factor = static_cast<IntegratedColoredGICPFactor*>(colored_gicp.get());
factor->set_fused_cov_cache_mode(FusedCovCacheMode::COMPACT);
factor->set_num_threads(4);

graph.add(colored_gicp);
```

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

### Run Demo Executables

Source: https://github.com/koide3/gtsam_points/blob/master/README.md

Navigate to the project directory and execute the provided demo binaries to explore different functionalities.

```bash
cd gtsam_points
./build/demo_matching_cost_factors
./build/demo_bundle_adjustment
./build/demo_continuous_time
./build/demo_continuous_trajectory
./build/demo_colored_registration
```

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

### Get Number of Voxels

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Returns the current count of voxels present in the map.

```cpp
size_t num_voxels() const;
```

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

### Create Point Cloud

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Initializes a point cloud on the CPU and adds points, normals, and covariances.

```cpp
auto cloud = std::make_shared<gtsam_points::PointCloudCPU>();
cloud->add_points(points);
cloud->add_normals(normals);
cloud->add_covariances(covs);
```

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

### Get Voxel Covariances

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Returns a vector containing the covariance matrices for each voxel in the map.

```cpp
std::vector<Eigen::Matrix4d> voxel_covs() const;
```

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

### Get Leaf Size

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Retrieves the current voxel size (leaf size) of the map.

```cpp
double leaf_size() const;
```

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

### ISAM2Ext Get Error

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Calculates the error of the factor graph at the given variable values.

```cpp
double getError(const gtsam::Values& values) const;
```

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

### Get Maximum Knot Index

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Returns the highest valid index for a knot in the trajectory.

```cpp
int knot_max_id() const;
```

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

### Get Knot Timestamp

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Retrieves the timestamp of a specific knot in the trajectory based on its index.

```cpp
double knot_stamp(int i) const;
```

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

### Get Voxel Intensities

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Returns a vector containing the mean intensity values for each voxel in the map.

```cpp
std::vector<double> voxel_intensities() const;
```

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

### Get Voxel Normals

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Returns a vector containing the mean normal vectors for each voxel in the map.

```cpp
std::vector<Eigen::Vector4d> voxel_normals() const;
```

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

### Import Paths for gtsam_points Optimization

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Include these headers for optimization algorithms, including extended Levenberg-Marquardt and ISAM2.

```cpp
#include <gtsam_points/optimizers/levenberg_marquardt_ext.hpp>
#include <gtsam_points/optimizers/isam2_ext.hpp>
```

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

### Get Point by Global Index

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Retrieves the point data associated with a given global index.

```cpp
decltype(auto) point(const size_t i) const;
```

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

### Get Inner Iterations Count

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Returns the number of inner iterations that have been completed during the optimization process.

```cpp
int getInnerIterations() const;
```

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

### Get Voxel Mean Positions

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Returns a vector containing the mean positions of all voxels currently in the map.

```cpp
std::vector<Eigen::Vector4d> voxel_points() const;
```

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

### Import Paths for gtsam_points Utilities

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Include these headers for utility functions such as continuous trajectory and stopwatch.

```cpp
#include <gtsam_points/util/continuous_trajectory.hpp>
#include <gtsam_points/util/stopwatch.hpp>
```

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

### Get Intensity by Global Index

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Retrieves the intensity value associated with a point at the given global index.

```cpp
decltype(auto) intensity(const size_t i) const;
```

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

### ISAM2Ext Constructor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Initializes the ISAM2Ext solver with optional custom parameters. If no parameters are provided, default parameters are used.

```APIDOC
## ISAM2Ext Constructor

### Description
Initializes the ISAM2Ext solver with optional custom parameters. If no parameters are provided, default parameters are used.

### Signature
```cpp 
ISAM2Ext(const gtsam_points::ISAM2ExtParams& params = gtsam_points::ISAM2ExtParams());
```
```

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

### Get Covariance by Global Index

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Retrieves the covariance matrix associated with a point at the given global index.

```cpp
decltype(auto) cov(const size_t i) const;
```

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

### Get Normal by Global Index

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Retrieves the normal vector associated with a point at the given global index.

```cpp
decltype(auto) normal(const size_t i) const;
```

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

### print

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Prints the optimizer state and parameters to the console.

```APIDOC
## print

### Description
Print optimizer state and parameters.

### Method
`print(const std::string& str = "") const`

### Parameters
* **str** (const std::string&) - Optional string to prepend to the output
```

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

### ISAM2Ext::print

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Prints the current state of the solver to the console, optionally with a preceding string.

```APIDOC
## ISAM2Ext::print

### Description
Prints the current state of the solver to the console, optionally with a preceding string.

### Signature
```cpp
void print(const std::string& s = "") const;
```

### Parameters

*   **s** (const std::string&, optional): A string to prepend to the output. Defaults to an empty string.
```

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

### Get Target Voxel Map

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Returns a constant reference to the target voxel map used by the factor.

```cpp
const std::shared_ptr<const GaussianVoxelMapCPU>& get_target() const;

```

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

### IntegratedColorConsistencyFactor Usage Example

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-specialized-factors.md

Adds both a geometric ICP factor and a color consistency factor to the graph for combined alignment.

```cpp
// Add geometric alignment
graph.add(icp_factor);

// Add color consistency constraint
graph.add(color_factor);
```

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

### Import Paths for gtsam_points Registration

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Include these headers for registration algorithms like RANSAC and graduated non-convexity.

```cpp
#include <gtsam_points/registration/ransac.hpp>
#include <gtsam_points/registration/graduated_non_convexity.hpp>
```

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

### Get Voxel Resolution

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-types.md

Retrieves the current voxel resolution of the map. This value determines the size of each voxel in meters.

```cpp
double voxel_resolution() const override;
```

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

### FastOccupancyGrid Constructor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Initializes a FastOccupancyGrid with a set of points, their count, and the desired resolution. Use this to create a new occupancy grid for point cloud processing.

```cpp
FastOccupancyGrid(
  const Eigen::Vector4d* points,
  int num_points,
  double resolution);
```

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

### Get Voxel Data as PointCloudCPU

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Retrieves all voxel data, including points and associated attributes, as a PointCloudCPU object.

```cpp
PointCloudCPU::Ptr voxel_data() const;
```

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

### Import Paths for gtsam_points Segmentation

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Include this header for segmentation algorithms like region growing.

```cpp
#include <gtsam_points/segmentation/region_growing.hpp>
```

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

### Get Current Damping Parameter

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Retrieves the current value of the damping parameter (Levenberg-Marquardt lambda) used in the optimization.

```cpp
double lambda() const;
```

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

### Typical GTSAM Points Registration Workflow

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

This snippet outlines the standard procedure for registering point clouds using GTSAM Points, from loading data to optimizing the estimated transform.

```cpp
// 1. Load point clouds
auto target = load_cloud("target.pcd");
auto source = load_cloud("source.pcd");

// 2. Compute normals (if needed)
auto target_normals = compute_normals(*target);
auto source_normals = compute_normals(*source);

// 3. Build spatial indices
auto target_tree = std::make_shared<gtsam_points::KdTree>(
  target->points, target->size(), 4
);
auto source_tree = std::make_shared<gtsam_points::KdTree>(
  source->points, source->size(), 4
);

// 4. (Optional) Extract features for global registration
auto target_fpfh = gtsam_points::estimate_fpfh(*target, *target_tree);
auto source_fpfh = gtsam_points::estimate_fpfh(*source, *source_tree);

// 5. (Optional) Global registration
gtsam_points::RANSACParams ransac_params;
auto global_result = gtsam_points::estimate_pose_ransac(
  *target, *source,
  target_fpfh.data(), source_fpfh.data(),
  *target_tree, *target_tree,  // Feature tree = spatial tree for FPFH
  ransac_params
);

// 6. Create factor graph
gtsam::NonlinearFactorGraph graph;
gtsam::Values init_values;

// Add prior
graph.add(gtsam::PriorFactor<gtsam::Pose3>(
  X(0),
  global_result.T_source_target,
  prior_noise
));

// Add registration factor
auto icp = std::make_shared<gtsam_points::IntegratedICPFactor>(
  X(0), X(1),
  target, source,
  target_tree
);
icp->set_max_correspondence_distance(1.0);
graph.add(icp);

init_values.insert(X(0), global_result.T_source_target);
init_values.insert(X(1), gtsam::Pose3());  // Identity for source

// 7. Optimize
auto optimizer = gtsam_points::LevenbergMarquardtOptimizerExt(
  graph, init_values
);
const auto& optimized = optimizer.optimize();

// 8. Extract result
auto estimated_transform = optimized.at<gtsam::Pose3>(X(0));
```

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

### Get Knot Index from Time

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Determines the knot index corresponding to a given time, useful for 4-knot B-spline basis.

```cpp
int knot_id(double t) const;
```

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

### ISAM2Ext Print Method

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Prints the current state of the ISAM2 solver.

```cpp
void print(const std::string& s = "") const;
```

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

### ContinuousTrajectory Constructor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Initializes a ContinuousTrajectory object with specified parameters for symbol, start time, end time, and knot interval.

```cpp
ContinuousTrajectory(
  char symbol,
  double start_time,
  double end_time,
  double knot_interval);
```

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

### Configure Quick RANSAC

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Set parameters for RANSAC (Random Sample Consensus) including maximum iterations, degrees of freedom (dof), number of threads, and a seed for reproducibility.

```cpp
params.max_iterations = 10000;
params.dof = 6;					// or 4
params.num_threads = 4;
params.seed = 12345;					// For reproducibility
```

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

### Initialize GaussianVoxelMapCPU

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-types.md

Constructs a GaussianVoxelMapCPU with a specified voxel resolution. This is the primary way to create an instance of the map.

```cpp
GaussianVoxelMapCPU(double resolution);
```

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

### FastOccupancyGrid Constructor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-ann.md

Initializes a FastOccupancyGrid with a set of points, specifying the resolution of the grid.

```APIDOC
## FastOccupancyGrid Constructor

### Description
Initializes a binary occupancy grid for efficient point cloud overlap estimation.

### Method
Constructor

### Parameters
- **points** (const Eigen::Vector4d*) - Pointer to an array of 4D points.
- **num_points** (int) - The number of points in the array.
- **resolution** (double) - The resolution of the voxel grid in meters.
```

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

### Configure Quick GNC

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Set parameters for GNC (Graduated Non-Convexity) including maximum iterations, degrees of freedom (dof), number of threads, and verbosity.

```cpp
params.max_iterations = 64;
params.dof = 6;
params.num_threads = 4;
params.verbose = true;
```

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

### Get Inlier Fraction

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Calculates the fraction of source points with valid voxel correspondences, ranging from 0.0 to 1.0. Call after linearization.

```cpp
double inlier_fraction() const;

```

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

### Import Paths for gtsam_points Nearest Neighbor Search

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Include these headers for nearest neighbor search functionalities, including KD-tree, incremental voxel maps, and fast occupancy grids.

```cpp
#include <gtsam_points/ann/kdtree.hpp>
#include <gtsam_points/ann/incremental_voxelmap.hpp>
#include <gtsam_points/ann/fast_occupancy_grid.hpp>
```

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

### Get Interpolated Pose from Values

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Calculates the interpolated pose at a specific time using optimized knot poses from GTSAM Values.

```cpp
gtsam::Pose3 pose(
  const gtsam::Values& values,
  double t);
```

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

### IntegratedVGICPFactor Constructors

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Provides the signatures for creating an IntegratedVGICPFactor. It can be initialized as a binary factor with two keys and associated data, or as a unary factor with a fixed target pose.

```APIDOC
## IntegratedVGICPFactor Constructors

### Binary Factor Constructor
```cpp
IntegratedVGICPFactor_(
  gtsam::Key target_key,
  gtsam::Key source_key,
  const GaussianVoxelMap::ConstPtr& target_voxels,
  const std::shared_ptr<const PointCloud>& source);
```

### Unary Factor Constructor
```cpp
IntegratedVGICPFactor_(
  const gtsam::Pose3& fixed_target_pose,
  gtsam::Key source_key,
  const GaussianVoxelMap::ConstPtr& target_voxels,
  const std::shared_ptr<const PointCloud>& source);
```
```

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

### Get Interpolated Pose Expression

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Provides a GTSAM expression for the interpolated pose at a given time, suitable for use in factor definitions.

```cpp
gtsam::Pose3_ pose(
  double t,
  const gtsam::Double_& t_);
```

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

### Configure IntegratedGICPFactor Settings

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/README.md

Shows how to customize an IntegratedGICPFactor by setting parameters like maximum correspondence distance, number of threads, and fused covariance cache mode. Use this pattern for other configurable components.

```cpp
auto factor = std::make_shared<IntegratedGICPFactor>(...);
factor->set_max_correspondence_distance(1.5);
factor->set_num_threads(4);
factor->set_fused_cov_cache_mode(FusedCovCacheMode::COMPACT);
```

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

### Get Number of Inliers

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Retrieves the count of source points that successfully found valid correspondences in the target voxel map. This should be called after linearization.

```cpp
int num_inliers() const;

```

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

### Initialize Region Growing

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-features-segmentation.md

Initializes the region growing process from a specified seed point. Requires the point cloud, a spatial search index, the seed point, and algorithm parameters.

```cpp
template <typename PointCloud>
RegionGrowingContext region_growing_init_(
  const PointCloud& points,
  const NearestNeighborSearch& search,
  const Eigen::Vector4d& seed_point,
  const RegionGrowingParams& params);

RegionGrowingContext region_growing_init(
  const PointCloud& points,
  const NearestNeighborSearch& search,
  const Eigen::Vector4d& seed_point,
  const RegionGrowingParams& params);
```

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

### Get inlier fraction from IntegratedGICPFactor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Retrieves the fraction of source points with valid correspondences within the set distance threshold. Must be called after linearization.

```cpp
double inlier_fraction() const;
```

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

### Import Paths for gtsam_points Types

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Include these headers to access point cloud and voxel map types for both CPU and GPU implementations.

```cpp
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/types/point_cloud_cpu.hpp>
#include <gtsam_points/types/gaussian_voxelmap_cpu.hpp>
```

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

### Get IMU Measurement Expression

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-utilities.md

Generates a GTSAM expression for linear acceleration and angular velocity at a given time, with an optional gravitational acceleration vector.

```cpp
gtsam::Vector6_ imu(
  double t,
  const gtsam::Double_& t_,
  const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665));
```

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

### Configure Quick FPFH

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Set parameters for Fast Point Feature Histograms (FPFH) estimation, including search radius and number of threads.

```cpp
gtsam_points::FPFHEstimationParams params;
params.search_radius = 10.0;					// 10 meters
params.num_threads = 4;
```

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

### Configure Quick GICP

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Configure the fused covariance cache mode for Generalized ICP (GICP). Options include COMPACT, FULL (fast but memory-intensive), and NONE (slow).

```cpp
factor->set_fused_cov_cache_mode(FusedCovCacheMode::COMPACT);
// or FULL (fast but memory), NONE (slow)
```

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

### Get Approximate Memory Usage

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Returns the approximate memory usage in bytes for the factor. This calculation excludes the memory occupied by external point cloud objects.

```cpp
size_t memory_usage() const override;
```

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

### Configure Quick Voxelmap

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/quick-reference.md

Set parameters for Voxelmap configuration, including voxel resolution, neighbor voxel mode, and Least Recently Used (LRU) settings for horizon and clear cycle.

```cpp
voxelmap->set_voxel_resolution(0.1);			// 10cm voxels
voxelmap->set_neighbor_voxel_mode(27);			// Full 27-neighbor
voxelmap->set_lru_horizon(1000);				// Delete after 1000 steps
voxelmap->set_lru_clear_cycle(100);				// Check every 100 insertions
```

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

### IntegratedVGICPFactor Parameters

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-factors.md

Details the parameters used in the IntegratedVGICPFactor constructors, including keys for poses, fixed poses, voxel maps, and source point clouds.

```APIDOC
## Parameters

| Parameter | Type | Description |
|-----------|------|-------------|
| target_key | gtsam::Key | Key for target pose |
| source_key | gtsam::Key | Key for source pose |
| fixed_target_pose | gtsam::Pose3 | Fixed target pose for unary factor |
| target_voxels | GaussianVoxelMap::ConstPtr | Voxelized target map with Gaussian statistics |
| source | std::shared_ptr<const PointCloud> | Source point cloud |
```

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

### Global Registration with Features

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/README.md

Estimate pose using RANSAC with FPFH features for global registration. Configure RANSAC parameters like max iterations and degrees of freedom.

```cpp
auto fpfh_features = estimate_fpfh(*cloud, *kdtree, fpfh_params);

RANSACParams ransac_params;
ransac_params.max_iterations = 10000;
ransac_params.dof = 6;

auto result = estimate_pose_ransac(
  *target, *source,
  target_fpfh.data(), source_fpfh.data(),
  *target_tree, *feature_tree,
  ransac_params
);
```

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

### Voxelized GICP for Speed

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/README.md

Utilize IntegratedVGICPFactor with a GaussianVoxelMapCPU for faster registration by voxelizing the target cloud.

```cpp
auto voxelmap = std::make_shared<GaussianVoxelMapCPU>(0.1);  // 10cm voxels
voxelmap->insert(*target_cloud);

auto vgicp = std::make_shared<IntegratedVGICPFactor>(
  x0, x1, voxelmap, source_cloud
);
graph.add(vgicp);
```

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

### Estimate FPFH Features

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-features-segmentation.md

This snippet shows how to prepare a point cloud and its normals, build a spatial index using a KdTree, and then estimate FPFH features for each point. Ensure that points and normals are loaded and computed before proceeding. The search radius and number of threads for feature estimation can be configured via FPFHEstimationParams.

```cpp
#include <gtsam_points/features/fpfh_estimation.hpp>
#include <gtsam_points/ann/kdtree.hpp>

// Prepare point cloud
std::vector<Eigen::Vector4d> points = load_points();
std::vector<Eigen::Vector4d> normals = compute_normals(points);

// Build spatial index
auto kdtree = std::make_shared<gtsam_points::KdTree>(
  points.data(), points.size(), 4
);

// Estimate FPFH features
gtsam_points::FPFHEstimationParams fpfh_params;
fpfh_params.search_radius = 10.0;    // 10m radius
fpfh_params.num_threads = 4;

auto features = gtsam_points::estimate_fpfh(
  points.data(),
  normals.data(),
  points.size(),
  *kdtree,
  fpfh_params
);

// features[i] is a 33-dimensional vector for point i
std::cout << "Feature dimension: " << features[0].size() << std::endl;
```

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

### GICP with Covariance

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/README.md

Employ IntegratedGICPFactor for GICP registration, enabling covariance caching for performance.

```cpp
auto gicp = std::make_shared<IntegratedGICPFactor>(
  x0, x1, target_cloud, source_cloud, target_tree
);
gicp->set_fused_cov_cache_mode(FusedCovCacheMode::FULL);
graph.add(gicp);
```

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

### LevenbergMarquardtOptimizerExt Constructors

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Constructors for LevenbergMarquardtOptimizerExt. One takes the graph, initial values, and optional parameters. The other additionally accepts a variable ordering for the linear solver.

```cpp
LevenbergMarquardtOptimizerExt(
  const gtsam::NonlinearFactorGraph& graph,
  const gtsam::Values& initialValues,
  const LevenbergMarquardtExtParams& params = LevenbergMarquardtExtParams());

LevenbergMarquardtOptimizerExt(
  const gtsam::NonlinearFactorGraph& graph,
  const gtsam::Values& initialValues,
  const gtsam::Ordering& ordering,
  const LevenbergMarquardtExtParams& params = LevenbergMarquardtExtParams());
```

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

### Add Scan Matching Factors to GTSAM Factor Graph

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/README.md

Demonstrates how to add ICP, GICP, and prior factors to a GTSAM NonlinearFactorGraph for joint optimization. Ensure GTSAM is properly installed and configured.

```cpp
gtsam::NonlinearFactorGraph graph;
graph.add(icp_factor);
graph.add(gicp_factor);
graph.add(prior_factor);

auto optimizer = LevenbergMarquardtOptimizerExt(graph, initial_values);
auto result = optimizer.optimize();
```

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

### GaussianVoxelMapCPU Configuration Methods

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/configuration.md

Set voxel resolution, LRU clear cycle, LRU horizon, and neighbor voxel mode for GaussianVoxelMapCPU.

```cpp
voxelmap->set_voxel_resolution(double leaf_size);
voxelmap->set_lru_clear_cycle(int cycle);
voxelmap->set_lru_horizon(int horizon);
voxelmap->set_neighbor_voxel_mode(int mode);
```

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

### ISAM2Ext Constructor

Source: https://github.com/koide3/gtsam_points/blob/master/_autodocs/api-reference-optimizers.md

Constructs an ISAM2Ext object with optional parameters.

```cpp
ISAM2Ext(const gtsam_points::ISAM2ExtParams& params = gtsam_points::ISAM2ExtParams());
```