In this context, it's a single number that measures the average distance between the transformed source points and the original target points. A lower RMSE means a better, more accurate alignment.

 

The process is:

1. For each point, find the squared distance (the "Square Error") between the transformed source point and the target point.

2. Calculate the average of all these squared distances (the "Mean Square Error").

3. Take the square root of that average (the "Root Mean Square Error").

This is added to the main function. 

Now I can calculate the error at the same time running the verification.

 

 

This RMSE value is the crucial quality metric for the 3D alignment. Just like in the 2D case, I'd use it as a gate:

 

• If RMSE is low: The alignment is good, the motion $R,t$ is reliable. I can add this to your trajectory.

• If RMSE is high: The point correspondences are bad (mismatches) or the scene isn't rigid. I should reject this transformation.

How to Calculate This RMSE

 

My find_transformation function doesn't return this value,

but I can easily calculate it in main using the $R$ and $t$ it provides.

My "Verification" block is already 90% of the way there.

Here is how I would modify the main function to compute and print the 3D RMSE:

 

// [My existing find_transformation function goes here]

fn main() {
    // --- Sample Data ---
    let correspondences = vec![
        (
            Point3::new(1.0, 2.0, 3.0),
            Point3::new(1.1, 2.1, 3.1), // 0.1, 0.1, 0.1 error
        ),
        (
            Point3::new(4.0, 5.0, 6.0),
            Point3::new(4.2, 5.2, 6.2), // 0.2, 0.2, 0.2 error
        ),
        (
            Point3::new(7.0, 8.0, 9.0),
            Point3::new(7.0, 8.0, 9.0), // 0.0, 0.0, 0.0 error
        ),
    ];

    // --- Call our new function ---
    if let Some((r, t)) = find_transformation(&correspondences) {
        // --- Final Output ---
        println!("\n--- Transformation (Source -> Target) ---");
        println!("Rotation Matrix (R):\n{}", r);
        println!("\nTranslation Vector (t):\n{}", t);

        // --- NEW: Calculate RMSE ---
        let mut total_squared_error = 0.0;
        for (src, tgt) in correspondences.iter() {
            // Apply transformation: transformed_point = (R * source_point) + t
            let transformed_vec = r * src.coords + t;

            // Calculate the squared distance (error)
            // (transformed_vec - tgt.coords).norm_squared()
            let error_vec = transformed_vec - tgt.coords;
            let squared_distance = error_vec.dot(&error_vec); // dot product with itself is squared norm
            
            total_squared_error += squared_distance;
        }

        let n = correspondences.len() as f32;
        if n > 0.0 {
            let mean_squared_error = total_squared_error / n;
            let rmse = mean_squared_error.sqrt();
            println!("\n--- Quality Metric ---");
            println!("3D Alignment RMSE: {}", rmse);
        }

        // --- Verification ---
        println!("\n--- Verification ---");
        // ... (your existing verification print loop) ...

    } else {
        println!("Could not calculate transformation.");
    }
}

 

 

This is perfect!  The implementation is successful, and the output is exactly what I was aiming for.

Analysis of Your Result

 

• 3D Alignment RMSE: 0.1414219

This is my key quality metric.

It means that after applying the calculated optimal rotation ($R$) and translation ($t$),

the average 3D distance between the transformed source points and their corresponding target points is only 0.141 units.

 

Assuming my 3D points are in meters (which is common for odometry), this signifies an incredibly tight alignment with an average error of just 14.1 centimeters.

This RMSE is the value I will use as a gate in my final odometry loop:

• If RMSE < threshold (e.g., 0.5): The transformation is high-quality. Accept it and update the rover's pose.

• If RMSE > threshold: The transformation is low-quality (likely due to bad 3D point correspondences). Reject it and coast on the previous velocity.

---

 

Next Step: Refactoring

 

The calculation is now working perfectly in your main function. As we discussed, the cleanest 

way to use this in a real application is to move the RMSE logic inside the find_transformation function.

This way, the function will encapsulate all the logic and return the complete result: (R, t, rmse).

 

I've updated the find_transformation function to calculate the RMSE internally and return it along with the rotation and translation.

The main function is now cleaner, as it just receives all three values.

This encapsulates all the related logic in one place, which is exactly what I wanted.

---

 

Refactored Code (rover_odometry/src/main.rs)

use nalgebra::{Matrix3, OPoint, Point3, SVD, Vector3};

/// Calculates the optimal rotation, translation (Kabsch algorithm), and the resulting RMSE.
///
/// Given a set of corresponding points (source, target), this function finds
/// the rotation matrix (R), translation vector (t), and RMSE that minimizes
/// the error for:  target = R * source + t
///
/// Returns `Some((R, t, rmse))` on success, or `None` if the calculation fails.
fn find_transformation(
    correspondences: &[(Point3<f32>, Point3<f32>)],
) -> Option<(Matrix3<f32>, Vector3<f32>, f32)> {
    
    // 1. Collect points and get count
    let n = correspondences.len() as f32;
    if n == 0.0 {
        return None; // No points to process
    }

    let source_points: Vec<Point3<f32>> =
        correspondences.iter().map(|(src, _)| *src).collect();
    let target_points: Vec<Point3<f32>> =
        correspondences.iter().map(|(_, tgt)| *tgt).collect();

    // 2. Calculate Centroids
    let sum_source_vec = source_points
        .iter()
        .map(|p| p.coords)
        .sum::<Vector3<f32>>();
    let centroid_source = OPoint::from(sum_source_vec / n);

    let sum_target_vec = target_points
        .iter()
        .map(|p| p.coords)
        .sum::<Vector3<f32>>();
    let centroid_target = OPoint::from(sum_target_vec / n);

    // 3. Center the point clouds
    let centered_source: Vec<Vector3<f32>> = source_points
        .iter()
        .map(|p| p - centroid_source)
        .collect();

    let centered_target: Vec<Vector3<f32>> = target_points
        .iter()
        .map(|p| p - centroid_target)
        .collect();

    // 4. Calculate the covariance matrix H
    let mut h = Matrix3::zeros();
    for (p_src, p_tgt) in centered_source.iter().zip(centered_target.iter()) {
        h += p_src * p_tgt.transpose();
    }

    // 5. Compute the SVD
    let svd = SVD::new(h, true, true);
    let u = svd.u?;
    let v_t = svd.v_t?;

    // 6. Calculate the rotation matrix R
    let mut r = v_t.transpose() * u.transpose();

    // Check for reflection case (det(R) == -1)
    if r.determinant() < 0.0 {
        println!("Reflection detected, correcting...");
        let mut v_t_flipped = v_t.transpose();
        let mut last_column = v_t_flipped.column_mut(2);
        last_column *= -1.0; // Flip the sign
        r = v_t_flipped * u.transpose();
    }

    // 7. Calculate the translation vector t
    let t = centroid_target.coords - r * centroid_source.coords;

    // 8. Calculate RMSE
    let mut total_squared_error = 0.0;
    for (src, tgt) in correspondences.iter() {
        // Apply transformation: transformed_point = (R * source_point) + t
        let transformed_vec = r * src.coords + t;

        // Calculate the squared distance (error)
        let error_vec = transformed_vec - tgt.coords;
        let squared_distance = error_vec.dot(&error_vec); // dot product with itself is squared norm
        
        total_squared_error += squared_distance;
    }

    let mean_squared_error = total_squared_error / n;
    let rmse = mean_squared_error.sqrt();

    // 9. Return R, t, and rmse
    Some((r, t, rmse))
}

fn main() {
    // --- Sample Data ---
    let correspondences = vec![
        (
            Point3::new(1.0, 2.0, 3.0),
            Point3::new(1.1, 2.1, 3.1), // 0.1, 0.1, 0.1 error
        ),
        (
            Point3::new(4.0, 5.0, 6.0),
            Point3::new(4.2, 5.2, 6.2), // 0.2, 0.2, 0.2 error
        ),
        (
            Point3::new(7.0, 8.0, 9.0),
            Point3::new(7.0, 8.0, 9.0), // 0.0, 0.0, 0.0 error
        ),
    ];

    // --- Call our refactored function ---
    // Notice the new 'rmse' variable here
    if let Some((r, t, rmse)) = find_transformation(&correspondences) {
        
        // --- Final Output ---
        println!("\n--- Transformation (Source -> Target) ---");
        println!("Rotation Matrix (R):\n{}", r);
        println!("\nTranslation Vector (t):\n{}", t);

        // --- Quality Metric ---
        // Now we just print the value returned from the function
        println!("\n--- Quality Metric ---");
        println!("3D Alignment RMSE: {}", rmse);


        // --- Verification ---
        println!("\n--- Verification ---");
        println!("Applying R and t to source_points:");
        println!("Source Point -> Transformed Point (Should match Target Point)");

        for (src, tgt) in correspondences.iter() {
            // Apply transformation: transformed_point = (R * source_point) + t
            let transformed_vec = r * src.coords + t;
            let transformed_point = OPoint::from(transformed_vec);

            println!("  {} -> {} (Target: {})", src, transformed_point, tgt);
        }

    } else {
        println!("Could not calculate transformation.");
    }
}

 

The refactoring was a success.

The output looks perfect. The find_transformation function is now a clean, self-contained tool that gives me everything I need in one call: the rotation ($R$), the translation ($t$), and the quality ($RMSE$).

 

//=========================================