ICP and Mesh Alignment

Comprehensive guide to point cloud and mesh alignment using ICP and MeshAlignmentAlgorithm.

Collaboration diagram for ICP and Mesh Alignment:

Comprehensive guide to point cloud and mesh alignment using ICP and MeshAlignmentAlgorithm.

This page provides detailed information and code examples for aligning point clouds and meshes using the Iterative Closest Point (ICP) algorithm and the MeshAlignmentAlgorithm class. These tools are essential for 3D registration tasks in computer vision applications.

ICP Class Overview

The ICP class provides a flexible implementation of the Iterative Closest Point algorithm for aligning point clouds. It supports both CPU and GPU acceleration, multiple matching strategies, and various termination criteria.

Key Features

• Multiple Matching Strategies: Nearest neighbor and projective matching
• GPU Acceleration: OpenCL-based acceleration for projective matching
• Flexible Termination: Multiple criteria for stopping the optimization
• 1-to-1 and 1-to-N Alignment: Support for aligning one point cloud to multiple targets
• Configurable Parameters: Distance thresholds, angle thresholds, overlap ratios

Matching Strategies

The ICP class supports two main matching strategies:

Nearest Neighbor Matching

• Works with any type of point cloud
• Finds the closest point in the target for each input point
• More robust but computationally expensive
• Requires normals on the input point cloud

Projective Matching

• Projects points to other views using camera intrinsics
• Requires dense point clouds and camera calibration
• Faster computation, especially with GPU acceleration
• Requires normals on the input point cloud

Basic ICP Usage

Basic Point Cloud Alignment

The following example demonstrates basic usage of the ICP class for aligning two point clouds:

#include <ImFusion/Base/PointCloud.h>
#include <ImFusion/Vision/ICP.h>
 
using namespace ImFusion;
 
// Create ICP instance with GPU acceleration
ICP icp(true);
 
// Set input and target point clouds
// Note: Input point cloud must have normals
icp.setPointClouds(inputPointCloud, targetPointCloud);
 
// Configure ICP parameters
icp.setMatchingStrategy(ICP::MatchingStrategy::NearestNeighbor);
icp.setDistanceThreshold(5.0);  // 5mm distance threshold
icp.setAngleThreshold(30.0);    // 30 degrees angle threshold
icp.setMinMatches(100);         // Require at least 100 matches
icp.setAbortIterations(50);     // Stop after 50 iterations
icp.setAbortParTol(1e-6);       // Stop if parameter change < 1e-6
 
// Perform alignment
mat4 transformation;
std::pair<double, int> rms;
bool success = icp.align(&transformation, &rms);
 
if (success)
{
    LOG_INFO("Alignment successful!");
    LOG_INFO("RMS error: " << rms.first);
    LOG_INFO("Matches: " << rms.second);
}

Aligning to Multiple Target Point Clouds

ICP can align one input point cloud to multiple target point clouds:

#include <ImFusion/Base/PointCloud.h>
#include <ImFusion/Vision/ICP.h>
 
#include <vector>
 
using namespace ImFusion;
 
// Create ICP instance
ICP icp(true);
 
// Prepare multiple target point clouds
std::vector<PointCloud*> targets = {target1, target2, target3};
 
// Set input and multiple targets
icp.setPointClouds(inputPointCloud, targets);
 
// Configure parameters
icp.setMatchingStrategy(ICP::MatchingStrategy::Projective);
icp.setDistanceThreshold(3.0);
icp.setAngleThreshold(45.0);
 
// Perform alignment
bool success = icp.align();
 
// The input point cloud matrix is updated with the result, when no matrix pointer is passed to the align method
mat4 trsansformation = inputPointCloud->matrix();

Using Preloaded Point Clouds

For better performance when working with the same point clouds repeatedly, you can preload them:

#include <ImFusion/Base/PointCloud.h>
#include <ImFusion/Vision/ICP.h>
 
#include <vector>
 
using namespace ImFusion;
 
// Create ICP instance
ICP icp(true);
 
// Preload point clouds to GPU
std::vector<PointCloud*> pointClouds = {cloud1, cloud2, cloud3, cloud4};
icp.loadPointClouds(pointClouds);
 
// Use indices to reference preloaded point clouds
icp.setPointClouds(0, 1);  // Align cloud1 to cloud2
 
// Perform alignment
bool success = icp.align();
 
// Align to multiple targets
std::vector<int> targetIndices = {2, 3};
icp.setPointClouds(0, targetIndices);  // Align cloud1 to cloud3 and cloud4
 
// Perform alignment
success = icp.align();

Advanced ICP Configuration

Parameter Tuning Guidelines

Distance Threshold

• Too small: May not find enough correspondences, leading to poor alignment
• Too large: May include incorrect correspondences, reducing accuracy
• Typical range: 1-10mm for medical data, 5-50mm for general 3D data
• Rule of thumb: Start with 5% of the point cloud's bounding box diagonal

Angle Threshold

• Purpose: Ensures normal vectors are similar between corresponding points
• Range: 0-180 degrees
• Typical values: 30-60 degrees
• Consideration: Larger values allow more flexibility, up to the point of ignoring orientation at 180 degrees, but may include incorrect matches

Minimum Matches

• Purpose: Ensures sufficient correspondences for reliable alignment
• Rule of thumb: At least 100-1000 matches depending on point cloud size
• Too few: Alignment may be unstable
• Too many: May slow down computation unnecessarily

Termination Criteria

// Set multiple termination criteria
icp.setAbortIterations(100);      // Stop after 100 iterations
icp.setAbortTime(30.0);           // Stop after 30 seconds
icp.setAbortParTol(1e-6);         // Stop if parameter change < 1e-6
 
// The algorithm stops when ANY of these criteria are met
bool success = icp.align();

Computing Alignment Error

// Compute RMS error after alignment
int matchedPoints;
std::vector<std::vector<int>> matches;
double rmsError = icp.rmsError(&matchedPoints, &matches, true);
 
LOG_INFO("RMS Error: " << rmsError);
LOG_INFO("Matched Points: " << matchedPoints);
 
// Access individual matches if needed
for (size_t i = 0; i < matches.size(); ++i) {
    for (int matchIdx : matches[i]) {
        // Process match
    }
}

MeshAlignmentAlgorithm Overview

The MeshAlignmentAlgorithm class provides a high-level interface for aligning meshes and point clouds using various ICP implementations. It supports multiple backends including PCL, Open3D, and custom implementations.

Key Features

• Multiple ICP Backends: PCL, Open3D, and custom implementations
• Global Alignment: Support for aligning multiple point clouds simultaneously
• Automatic Preprocessing: Downsampling, normal estimation, and filtering
• Flexible Configuration: Extensive parameter tuning options
• Error Analysis: Built-in error computation and reporting

Available Algorithms

PCL Algorithms

• PointToPointICP_PCL: Basic point-to-point ICP
• PointToPlaneICP_PCL: Point-to-plane ICP (requires normals)
• GICP_PCL: Generalized ICP
• GICP_PCL_Color: Generalized ICP with color information

Open3D Algorithms

• PointToPointICP_Open3D: Point-to-point ICP
• PointToPlaneICP_Open3D: Point-to-plane ICP (requires normals)
• ColoredICP_Open3D: Colored ICP using RGB information
• RANSACGlobalRegistration_Open3D: RANSAC-based global registration
• FastGlobalRegistration_Open3D: Fast global registration

Custom Algorithms

• NearestNeighborPointToPlaneICP: Custom nearest neighbor implementation
• ProjectivePointToPlaneICP: Custom projective implementation
• GlobalICP: Global alignment with multiple strategies
• ManualCorrespondences: Manual point correspondence alignment

Basic MeshAlignmentAlgorithm Usage

Basic Point Cloud Alignment

#include <ImFusion/Vision/MeshAlignmentAlgorithm.h>
#include <ImFusion/Base/PointCloud.h>
#include <ImFusion/Base/DataList.h>
 
using namespace ImFusion;
 
// Prepare data list with point clouds
DataList dataList;
dataList.add(inputPointCloud);
dataList.add(targetPointCloud);
 
// Create mesh alignment algorithm
MeshAlignmentAlgorithm alignment(dataList);
 
// Configure algorithm
alignment.setICPAlgorithm(MeshAlignmentAlgorithm::ICPAlgorithm::PointToPlaneICP_PCL);
alignment.setMaxICPIterations(50);
alignment.setMaxCorrespondenceDistance(5.0);
alignment.setMaxCorrespondenceAngle(30.0);
alignment.setAbortParameterTolerance(1e-6);
 
// Run alignment
alignment.compute();
 
// Check results
if (alignment.inputPointCloud()) {
    LOG_INFO("Alignment completed successfully!");
    
    // Access aligned point cloud
    PointCloud* alignedCloud = alignment.inputPointCloud();
    
    // Compute RMS error if supported
    double rmsError = alignment.computeRMS();
    LOG_INFO("RMS Error: " << rmsError);
}

Aligning Multiple Point Clouds

#include <ImFusion/Vision/MeshAlignmentAlgorithm.h>
#include <ImFusion/Base/PointCloud.h>
#include <ImFusion/Base/DataList.h>
 
using namespace ImFusion;
 
// Prepare data list with multiple point clouds
DataList dataList;
dataList.add(cloud1);
dataList.add(cloud2);
dataList.add(cloud3);
dataList.add(cloud4);
 
// Create mesh alignment algorithm
MeshAlignmentAlgorithm alignment(dataList);
 
// Use global ICP for multiple point clouds
alignment.setICPAlgorithm(MeshAlignmentAlgorithm::ICPAlgorithm::GlobalICP);
alignment.setGlobalICPAlgorithm(MeshAlignmentAlgorithm::GlobalICPAlgorithm::FullProjectiveICPWithErrorCorrection);
alignment.setGlobalICPIterations(3);
alignment.setGlobalICPMinOverlap(0.3);
alignment.setAlignAcrossLoop(true);
 
// Configure parameters
alignment.setMaxICPIterations(30);
alignment.setMaxCorrespondenceDistance(10.0);
alignment.setMaxCorrespondenceAngle(45.0);
 
// Run alignment
alignment.compute();
 
// Get RMS errors for each point cloud
auto rmsErrors = alignment.rmsErrorsBeforeAndAfterAlignment();
for (size_t i = 0; i < rmsErrors.size(); ++i) {
    LOG_INFO("Cloud " << i << ": RMS before=" << rmsErrors[i].first 
              << ", RMS after=" << rmsErrors[i].second);
}

Advanced MeshAlignmentAlgorithm Configuration

Parameter Tuning Guidelines

Maximum Correspondence Distance

• Purpose: Limits the maximum distance for point correspondences
• Typical range: 1-50mm depending on point cloud scale and noise level
• Too small: May not find enough correspondences
• Too large: May include incorrect correspondences

Maximum Correspondence Angle

• Purpose: Ensures normal vectors are similar between corresponding points
• Range: 0-180 degrees
• Typical values: 30-60 degrees
• Consideration: Only relevant for point-to-plane ICP variants

Global ICP Configuration

#include <ImFusion/Vision/MeshAlignmentAlgorithm.h>
 
using namespace ImFusion;
 
// Configure for global alignment
MeshAlignmentAlgorithm alignment(dataList);
 
// Set global ICP algorithm
alignment.setICPAlgorithm(MeshAlignmentAlgorithm::ICPAlgorithm::GlobalICP);
alignment.setGlobalICPAlgorithm(MeshAlignmentAlgorithm::GlobalICPAlgorithm::IterativeGlobalICP);
 
// Global ICP specific parameters
alignment.setGlobalICPIterations(5);           // Number of global iterations
alignment.setGlobalICPMinOverlap(0.4);         // Minimum overlap ratio
alignment.setAlignAcrossLoop(true);            // Close the loop
alignment.setComputeGlobalAlignmentError(true); // Compute error statistics
 
// Standard ICP parameters
alignment.setMaxICPIterations(50);
alignment.setMaxCorrespondenceDistance(5.0);
alignment.setMaxCorrespondenceAngle(30.0);
 
// Run alignment
alignment.compute();

Manual Point Correspondences

#include <ImFusion/Vision/MeshAlignmentAlgorithm.h>
#include <ImFusion/Base/PointCloud.h>
#include <ImFusion/Base/DataList.h>
 
using namespace ImFusion;
 
// Prepare data list
DataList dataList;
dataList.add(sourcePointCloud);
dataList.add(targetPointCloud);
 
// Create alignment algorithm
MeshAlignmentAlgorithm alignment(dataList);
 
// Set manual correspondence mode
alignment.setICPAlgorithm(MeshAlignmentAlgorithm::ICPAlgorithm::ManualCorrespondences);
 
// Define corresponding points
std::vector<std::vector<vec3>> correspondences;
correspondences.push_back({vec3(0, 0, 0), vec3(1, 0, 0), vec3(0, 1, 0)});  // Source points
correspondences.push_back({vec3(1, 1, 1), vec3(2, 1, 1), vec3(1, 2, 1)});  // Target points
 
// Set correspondences
alignment.setPointCorrespondences(correspondences);
 
// Run alignment
alignment.compute();

Error Analysis and Validation

RMS Error Computation

// Compute RMS error after alignment
double rmsError = alignment.computeRMS();
 
// Get detailed error information
int matchedPoints;
std::vector<std::vector<int>> matches;
double weightedRmsError = alignment.computeRMS(&matchedPoints, &matches, true);
 
LOG_INFO("RMS Error: " << rmsError);
LOG_INFO("Weighted RMS Error: " << weightedRmsError);
LOG_INFO("Matched Points: " << matchedPoints);
 
// For global alignment, get errors for each point cloud
auto rmsErrors = alignment.rmsErrorsBeforeAndAfterAlignment();
for (size_t i = 0; i < rmsErrors.size(); ++i) {
    LOG_INFO("Point Cloud " << i << ":");
    LOG_INFO("  RMS before alignment: " << rmsErrors[i].first);
    LOG_INFO("  RMS after alignment: " << rmsErrors[i].second);
}

Input Data Validation

#include <ImFusion/Vision/MeshAlignmentAlgorithm.h>
 
// Check input data properties before alignment
bool hasNormals, hasTargetNormals, hasK, hasColors;
MeshAlignmentAlgorithm::inputDataInfo(
    hasNormals, hasTargetNormals, hasK, hasColors, dataList);
 
LOG_INFO("Input data properties:");
LOG_INFO("  Has normals: " << (hasNormals ? "Yes" : "No"));
LOG_INFO("  Has target normals: " << (hasTargetNormals ? "Yes" : "No"));
LOG_INFO("  Has camera intrinsics: " << (hasK ? "Yes" : "No"));
LOG_INFO("  Has colors: " << (hasColors ? "Yes" : "No"));
 
// Choose appropriate algorithm based on data properties
if (hasNormals && hasTargetNormals) {
    alignment.setICPAlgorithm(MeshAlignmentAlgorithm::ICPAlgorithm::PointToPlaneICP_PCL);
} else if (hasColors) {
    alignment.setICPAlgorithm(MeshAlignmentAlgorithm::ICPAlgorithm::ColoredICP_Open3D);
} else {
    alignment.setICPAlgorithm(MeshAlignmentAlgorithm::ICPAlgorithm::PointToPointICP_PCL);
}

Best Practices

Preprocessing Recommendations

1. Normal Estimation: Ensure input point clouds have accurate normals for point-to-plane ICP
2. Outlier Removal: Remove noise and outliers before alignment
3. Initial Alignment: Provide a reasonable initial transformation if point clouds are far apart

Parameter Selection Guidelines

1. Start Conservative: Begin with conservative parameters and gradually relax them
2. Monitor Convergence: Check iteration count and RMS error to ensure proper convergence
3. Validate Results: Always validate alignment results visually or through error metrics
4. Consider Data Properties: Choose algorithms based on available data (normals, colors, etc.)

Performance Optimization

1. GPU Acceleration: Use GPU acceleration when available for projective ICP
2. Preloading: Preload point clouds for repeated alignments
3. Algorithm Selection: Choose the most appropriate algorithm for your data and requirements

Troubleshooting

Common Issues and Solutions

No Convergence

• Symptoms: High RMS error, many iterations without improvement
• Solutions:
  • Increase distance threshold
  • Provide better initial alignment
  • Check for outliers in point clouds
  • Verify point cloud quality and overlap

Poor Alignment Quality

• Symptoms: High final RMS error, visually poor alignment
• Solutions:
  • Reduce distance threshold
  • Use point-to-plane ICP if normals are available
  • Try different matching strategies
  • Check for systematic errors in point clouds

Slow Performance

• Symptoms: Long computation times
• Solutions:
  • Use GPU acceleration if available
  • Reduce maximum iterations
  • Use faster algorithms (e.g., point-to-point instead of point-to-plane)

See also

ICP Class, MeshAlignmentAlgorithm Class