In manipulation, grasping and placing often require a full 6D pose estimate of an object: a rotation and translation describing the object frame relative to the robot or world frame. We assume the robot has acquired a 3D point cloud of the scene (e.g., from a depth camera or LiDAR) and possibly a model point cloud of the target object.

Let \( \mathcal{P} = \{p_i \in \mathbb{R}^3\}_{i=1}^N \) denote a model point cloud expressed in the object coordinate frame, and \( \mathcal{Q} = \{q_i \in \mathbb{R}^3\}_{i=1}^N \) a set of corresponding points in the scene frame. A 3D rigid-body pose is represented by a homogeneous transform

\[ T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}, \quad R \in SO(3),\; t \in \mathbb{R}^3, \]

where \( R \) is a rotation matrix with orthonormal columns and determinant one, and \( t \) is a translation vector. For a point \( p_i \) in object coordinates, its position in the scene frame is given by

\[ q_i \approx R p_i + t. \]

The core pose estimation problem under known correspondences is:

\[ (R^* , t^* ) = \arg\min_{R \in SO(3),\, t \in \mathbb{R}^3} \sum_{i=1}^N \left\| R p_i + t - q_i \right\|^2. \]

In practice, correspondences must often be estimated from partially overlapping, noisy, and cluttered point clouds. The ICP algorithm and its variants perform this joint correspondence and pose estimation.

flowchart TD
  A["Depth sensor / stereo"] --> B["Raw scene cloud"]
  B --> C["Pre-process (filter, downsample, segment)"]
  C --> D["Registration with model cloud"]
  D --> E["Estimated pose R,t"]
  E --> F["Grasp / manipulation planning"]
        

Assume we know correct point correspondences, i.e., each \( p_i \) matches \( q_i \) for \( i = 1,\dots,N \). Consider the cost function

\[ J(R,t) = \sum_{i=1}^N \left\| R p_i + t - q_i \right\|^2. \]

We first derive the optimal translation for a fixed rotation, and then solve for the optimal rotation. Define centroids

\[ \bar{p} = \frac{1}{N} \sum_{i=1}^N p_i, \quad \bar{q} = \frac{1}{N} \sum_{i=1}^N q_i, \]

and centered points

\[ \tilde{p}_i = p_i - \bar{p}, \quad \tilde{q}_i = q_i - \bar{q}. \]

Expanding the cost gives

\[ \begin{aligned} J(R,t) &= \sum_{i=1}^N \left\| R p_i + t - q_i \right\|^2 \\ &= \sum_{i=1}^N \left\| R \tilde{p}_i + R \bar{p} + t - \tilde{q}_i - \bar{q} \right\|^2. \end{aligned} \]

Let \( \delta = t + R \bar{p} - \bar{q} \). Using the fact that rotations preserve norms,

\[ J(R,t) = \sum_{i=1}^N \left\| R \tilde{p}_i - \tilde{q}_i + \delta \right\|^2. \]

Differentiating with respect to \( \delta \) and setting to zero shows that the optimum satisfies

\[ \delta^* = 0 \quad \Rightarrow \quad t^* = \bar{q} - R \bar{p}. \]

Substituting \( t^* \) back into the cost yields

\[ J(R,t^* ) = \sum_{i=1}^N \left\| R \tilde{p}_i - \tilde{q}_i \right\|^2. \]

Define the cross-covariance matrix

\[ H = \sum_{i=1}^N \tilde{p}_i \tilde{q}_i^\top \in \mathbb{R}^{3 \times 3}. \]

Using standard trace identities, the cost can be written as

\[ \begin{aligned} J(R,t^* ) &= \sum_{i=1}^N \left( \left\| \tilde{p}_i \right\|^2 + \left\| \tilde{q}_i \right\|^2 - 2\, \tilde{q}_i^\top R \tilde{p}_i \right) \\ &= \text{const} - 2\,\mathrm{tr}(R H), \end{aligned} \]

where the constant term does not depend on \( R \). Hence minimizing \( J(R,t^* ) \) is equivalent to

\[ R^* = \arg\max_{R \in SO(3)} \mathrm{tr}(R H). \]

Let the singular value decomposition of \( H \) be \( H = U \Sigma V^\top \), where \( U, V \in SO(3) \) and \( \Sigma = \mathrm{diag}(\sigma_1, \sigma_2, \sigma_3) \) with nonnegative singular values.

For any \( R \in SO(3) \), write \( R = V R' U^\top \) with \( R' \in SO(3) \) (since products of rotations remain rotations). Then

\[ \begin{aligned} \mathrm{tr}(R H) &= \mathrm{tr}(V R' U^\top U \Sigma V^\top) \\ &= \mathrm{tr}(V R' \Sigma V^\top) \\ &= \mathrm{tr}(R' \Sigma), \end{aligned} \]

using cyclic invariance of the trace and orthogonality of \( U, V \). Thus the maximization reduces to

\[ \max_{R' \in SO(3)} \mathrm{tr}(R' \Sigma). \]

Since \( \Sigma \) is diagonal, write \( R' = [r'_{ij}] \) and observe

\[ \mathrm{tr}(R' \Sigma) = \sigma_1 r'_{11} + \sigma_2 r'_{22} + \sigma_3 r'_{33}. \]

Under the constraints that \( R' \) is a rotation (orthonormal columns and determinant one), the trace is maximized when \( R' = I \), except for the case where enforcing \( \det(R') = 1 \) requires flipping a sign. The resulting optimal rotation is

\[ R^* = V \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(V U^\top) \end{bmatrix} U^\top, \]

and the optimal translation is

\[ t^* = \bar{q} - R^* \bar{p}. \]

This SVD-based solution is numerically stable and forms the core of many model-based pose estimation algorithms and ICP inner loops.

When correspondences are unknown, the ICP algorithm alternates between correspondence estimation and rigid registration. Let \( \mathcal{P} \) be transformed source points and \( \mathcal{Q} \) be a fixed target cloud.

Basic ICP loop:

1. Initialize \( T^{(0)} = (R^{(0)}, t^{(0)}) \) (e.g., from a coarse detector).
2. Correspondence step: For each transformed source point \( \hat{p}_i = R^{(k)} p_i + t^{(k)} \), find the closest point \( q_{\pi^{(k)}(i)} \in \mathcal{Q} \) in Euclidean distance.
3. Registration step: Compute \( (R^{(k+1)}, t^{(k+1)}) \) by solving the SVD registration between pairs \( (p_i, q_{\pi^{(k)}(i)}) \).
4. Check convergence (e.g., small change in cost or pose). If converged, stop; otherwise set \( k \leftarrow k+1 \) and repeat.

flowchart TD
  S["Initial pose guess T0"] --> T1["Transform source cloud"]
  T1 --> NN["Nearest-neighbor search for correspondences"]
  NN --> REG["Rigid registration via SVD"]
  REG --> UPD["Update pose Tk+1"]
  UPD --> CHK["Convergence test"]
  CHK -->|no| T1
  CHK -->|yes| OUT["Final pose estimate"]
        

ICP is guaranteed to monotonically decrease the cost \( \sum_i \left\| R p_i + t - q_{\pi(i)} \right\|^2 \) but only converges to a local minimum. Good initialization and appropriate variants (point-to-plane, generalized ICP) are crucial in practice.

Real point clouds contain occlusions, clutter, and sensor artifacts, so many correspondences are incorrect. Pure least-squares is highly sensitive to outliers. Robust approaches modify either the cost function or the correspondence selection.

Trimming: One strategy keeps only a fraction of the smallest residuals at each iteration. Let \( \rho_i(R,t) = \left\| R p_i + t - q_{\pi(i)} \right\| \) and let \( \mathcal{I}_K \) be the indices of the \( K \) smallest residuals. The trimmed objective is

\[ J_{\mathrm{trim}}(R,t) = \sum_{i \in \mathcal{I}_K} \left\| R p_i + t - q_{\pi(i)} \right\|^2. \]

M-estimators: Another approach replaces squared error with a robust penalty \( \rho(\cdot) \), such as the Huber or Tukey function:

\[ J_{\rho}(R,t) = \sum_{i=1}^N \rho\!\left( \left\| R p_i + t - q_{\pi(i)} \right\| \right). \]

These can be solved by iteratively reweighted least-squares, where each correspondence is weighted according to the derivative of \( \rho \).

RANSAC-based registration: We may repeatedly sample minimal sets of correspondences (e.g., three non-collinear pairs), compute a candidate transform using the SVD solution, and score it by the number of inliers whose residual is below a threshold. The transform with the maximum inlier count is then refined by least-squares on all inliers.

Least-squares registration has a natural probabilistic interpretation. Suppose that, given the true pose \( (R,t) \), each target point is generated as

\[ q_i = R p_i + t + \epsilon_i, \quad \epsilon_i \sim \mathcal{N}(0, \sigma^2 I_3),\ \text{independent}. \]

The likelihood of observing \( \{q_i\} \) given \( (R,t) \) is

\[ p(\{q_i\} \mid R,t) = \prod_{i=1}^N \mathcal{N}\!\left( q_i \mid R p_i + t,\; \sigma^2 I_3 \right). \]

The negative log-likelihood (up to an additive constant) is

\[ - \log p(\{q_i\} \mid R,t) = \frac{1}{2 \sigma^2} \sum_{i=1}^N \left\| R p_i + t - q_i \right\|^2 + \text{const}. \]

Therefore, the maximum likelihood estimate of \( (R,t) \) coincides with the least-squares solution. Robust penalties correspond to heavy-tailed noise models, such as Laplace or Student-t distributions.

This probabilistic view is useful for integrating pose estimation with Bayesian perception modules and for reasoning about uncertainty in downstream planning.

Python, together with numpy and open3d, provides convenient tools for implementing ICP and rigid registration. Below we show (i) a minimal SVD-based rigid transform solver, and (ii) how to call ICP from Open3D.

import numpy as np
import open3d as o3d

def rigid_transform_svd(P, Q):
    """
    Solve for R, t that minimize sum ||R p_i + t - q_i||^2
    P, Q: (N, 3) numpy arrays of corresponding points.
    """
    assert P.shape == Q.shape
    N = P.shape[0]

    # Compute centroids
    p_bar = P.mean(axis=0)
    q_bar = Q.mean(axis=0)

    # Center the points
    P_centered = P - p_bar
    Q_centered = Q - q_bar

    # Cross-covariance matrix H
    H = P_centered.T @ Q_centered

    # SVD of H
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T

    # Handle possible reflection
    if np.linalg.det(R) < 0:
        Vt[2, :] *= -1.0
        R = Vt.T @ U.T

    t = q_bar - R @ p_bar
    return R, t

# Example: using Open3D for ICP with point-to-point metric
source = o3d.io.read_point_cloud("model_cloud.pcd")
target = o3d.io.read_point_cloud("scene_cloud.pcd")

# Downsample for speed
source_ds = source.voxel_down_sample(voxel_size=0.005)
target_ds = target.voxel_down_sample(voxel_size=0.005)

# Initial guess (identity)
init_transform = np.eye(4)

threshold = 0.02  # max correspondence distance in meters
icp_result = o3d.pipelines.registration.registration_icp(
    source_ds,
    target_ds,
    threshold,
    init_transform,
    o3d.pipelines.registration.TransformationEstimationPointToPoint()
)

print("ICP fitness:", icp_result.fitness)
print("ICP inlier_rmse:", icp_result.inlier_rmse)
print("Estimated transform:\n", icp_result.transformation)
      

In manipulation systems, the resulting transformation matrix can be converted into a pose message (e.g., in ROS) and composed with known calibration transforms to map into the robot base frame.

The Point Cloud Library (PCL) offers high-performance ICP and registration routines. Below is a simplified example of point-to-point ICP for two point clouds:

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>

int main(int argc, char** argv)
{
    using PointT = pcl::PointXYZ;

    pcl::PointCloud<PointT>::Ptr source(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr target(new pcl::PointCloud<PointT>);

    if (pcl::io::loadPCDFile<PointT>("model_cloud.pcd", *source) < 0 ||
        pcl::io::loadPCDFile<PointT>("scene_cloud.pcd", *target) < 0)
    {
        std::cerr << "Error loading clouds" << std::endl;
        return -1;
    }

    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setInputSource(source);
    icp.setInputTarget(target);

    icp.setMaxCorrespondenceDistance(0.02);
    icp.setMaximumIterations(50);
    icp.setTransformationEpsilon(1e-8);
    icp.setEuclideanFitnessEpsilon(1e-6);

    pcl::PointCloud<PointT> aligned;
    icp.align(aligned);

    std::cout << "ICP has converged: " << icp.hasConverged()
              << " score: " << icp.getFitnessScore() << std::endl;

    Eigen::Matrix4f T = icp.getFinalTransformation();
    std::cout << "Estimated transform:\n" << T << std::endl;

    return 0;
}
      

For fine-grained control, PCL also supports point-to-plane ICP, generalized ICP, and robust rejection policies (e.g., trimmed or RANSAC-based).

While Java has fewer robotics-focused libraries, we can implement the SVD-based rigid transform with a numerical linear algebra package such as EJML or Apache Commons Math. Below is a sketch using EJML's SimpleMatrix interface.

import org.ejml.simple.SimpleMatrix;
import java.util.List;

public class RigidRegistration {

    public static class Point3D {
        public double x, y, z;
        public Point3D(double x, double y, double z) {
            this.x = x; this.y = y; this.z = z;
        }
    }

    public static class RigidTransform {
        public SimpleMatrix R; // 3x3
        public SimpleMatrix t; // 3x1
    }

    public static RigidTransform estimate(List<Point3D> P, List<Point3D> Q) {
        int N = P.size();
        if (N != Q.size()) {
            throw new IllegalArgumentException("Point lists must have same size");
        }

        // Compute centroids
        double px = 0, py = 0, pz = 0;
        double qx = 0, qy = 0, qz = 0;
        for (int i = 0; i < N; ++i) {
            px += P.get(i).x; py += P.get(i).y; pz += P.get(i).z;
            qx += Q.get(i).x; qy += Q.get(i).y; qz += Q.get(i).z;
        }
        px /= N; py /= N; pz /= N;
        qx /= N; qy /= N; qz /= N;

        // Build centered matrices (3 x N)
        SimpleMatrix Pm = new SimpleMatrix(3, N);
        SimpleMatrix Qm = new SimpleMatrix(3, N);
        for (int i = 0; i < N; ++i) {
            Point3D p = P.get(i);
            Point3D q = Q.get(i);
            Pm.set(0, i, p.x - px);
            Pm.set(1, i, p.y - py);
            Pm.set(2, i, p.z - pz);
            Qm.set(0, i, q.x - qx);
            Qm.set(1, i, q.y - qy);
            Qm.set(2, i, q.z - qz);
        }

        // Cross-covariance H = Pm * Qm^T
        SimpleMatrix H = Pm.mult(Qm.transpose());

        // SVD of H
        SimpleMatrix U = new SimpleMatrix(3, 3);
        SimpleMatrix W = new SimpleMatrix(3, 3);
        SimpleMatrix Vt = new SimpleMatrix(3, 3);
        H.svd(U, W, Vt);

        SimpleMatrix R = Vt.transpose().mult(U.transpose());

        // Handle reflection if det(R) < 0
        double detR = R.determinant();
        if (detR < 0) {
            // Flip last column of V
            Vt.set(2, 0, -Vt.get(2, 0));
            Vt.set(2, 1, -Vt.get(2, 1));
            Vt.set(2, 2, -Vt.get(2, 2));
            R = Vt.transpose().mult(U.transpose());
        }

        // t = q_bar - R * p_bar
        SimpleMatrix p_bar = new SimpleMatrix(3, 1, true, new double[]{px, py, pz});
        SimpleMatrix q_bar = new SimpleMatrix(3, 1, true, new double[]{qx, qy, qz});
        SimpleMatrix t = q_bar.minus(R.mult(p_bar));

        RigidTransform T = new RigidTransform();
        T.R = R;
        T.t = t;
        return T;
    }
}
      

This core alignment routine can be integrated into a Java-based ICP loop where nearest neighbors are found using a k-d tree or spatial index.

MATLAB's Computer Vision and Lidar toolboxes provide high-level functions such as pcregistericp and pcregisterndt. Below is a script that estimates pose with ICP and also implements the SVD-based alignment directly.

% Load point clouds
model = pcread("model_cloud.pcd");
scene = pcread("scene_cloud.pcd");

% Downsample
voxelSize = 0.005;
modelDS = pcdownsample(model, "gridAverage", voxelSize);
sceneDS = pcdownsample(scene, "gridAverage", voxelSize);

% ICP registration (point-to-point)
[tform, movingReg, rmse] = pcregistericp( ...
    modelDS, sceneDS, ...
    "Metric", "pointToPoint", ...
    "MaxIterations", 50, ...
    "Tolerance", [1e-6, 1e-6]);

R_icp = tform.T(1:3, 1:3);
t_icp = tform.T(1:3, 4);

disp("ICP rotation:");
disp(R_icp);
disp("ICP translation:");
disp(t_icp);

% SVD-based rigid transform from explicit correspondences
function [R, t] = rigidTransformSVD(P, Q)
% P, Q: N-by-3 matrices of corresponding points
assert(all(size(P) == size(Q)), "P and Q must have same size");
N = size(P, 1);

p_bar = mean(P, 1);
q_bar = mean(Q, 1);

P_centered = P - p_bar;
Q_centered = Q - q_bar;

H = P_centered' * Q_centered;
[U, S, V] = svd(H);

R = V * U';
if det(R) < 0
    V(:, 3) = -V(:, 3);
    R = V * U';
end

t = q_bar' - R * p_bar';
end
      

In Simulink, one typical approach is to encapsulate the SVD-based alignment or ICP step in a MATLAB Function block, taking point clouds and an initial transform as inputs and outputting the updated pose at each control cycle.

Wolfram Mathematica can manipulate matrices symbolically and numerically. The SVD-based rigid transform can be implemented compactly using built-in linear algebra operations.

(* P and Q are lists of 3D points, e.g., { {x1,y1,z1}, {x2,y2,z2}, ... } *)

rigidTransformSVD[P_, Q_] := Module[
  {n, pBar, qBar, Pcen, Qcen, H, u, s, v, R, t},

  n = Length[P];
  If[n != Length[Q], Throw["P and Q must have same length"]];

  pBar = Mean[P];
  qBar = Mean[Q];

  Pcen = (# - pBar) & /@ P;
  Qcen = (# - qBar) & /@ Q;

  H = Transpose[Pcen].Qcen;
  {u, s, v} = SingularValueDecomposition[H];

  R = v.Transpose[u];
  If[Det[R] < 0,
    v[[All, 3]] = -v[[All, 3]];
    R = v.Transpose[u];
  ];

  t = qBar - (R.pBar);

  << "Return as association"
  << | "Rotation" -> R, "Translation" -> t |
]

(* Example usage:
   P = { {0,0,0}, {1,0,0}, {0,1,0} };
   Q = { {1,1,0}, {2,1,0}, {1,2,0} };
   rigidTransformSVD[P, Q]
*)
      

Mathematica's symbolic capabilities can also be used to derive variants of the objective (e.g., with anisotropic covariances) and verify algebraic properties of the optimal solution.

Problem 1 (Optimal Translation Derivation): For fixed rotation \( R \), derive the expression for the optimal translation \( t^* \) that minimizes \( J(R,t) = \sum_{i=1}^N \left\| R p_i + t - q_i \right\|^2 \).

Solution:

Expand the cost:

\[ J(R,t) = \sum_{i=1}^N \left( R p_i + t - q_i \right)^\top \left( R p_i + t - q_i \right). \]

Differentiate with respect to \( t \) and set the gradient to zero:

\[ \nabla_t J(R,t) = 2 \sum_{i=1}^N \left( R p_i + t - q_i \right) = 0. \]

Solving gives

\[ N t + \sum_{i=1}^N R p_i - \sum_{i=1}^N q_i = 0 \quad \Rightarrow \quad t^* = \bar{q} - R \bar{p}, \]

where \( \bar{p} \) and \( \bar{q} \) are centroids.

Problem 2 (Trace Form of the Cost): Show that, after centering the point sets, the registration cost can be written as \( J(R,t^* ) = \text{const} - 2\,\mathrm{tr}(R H) \) with \( H = \sum_i \tilde{p}_i \tilde{q}_i^\top \).

Solution:

Using centered points, we have

\[ J(R,t^* ) = \sum_{i=1}^N \left\| R \tilde{p}_i - \tilde{q}_i \right\|^2. \]

Expand the squared norm:

\[ \begin{aligned} \left\| R \tilde{p}_i - \tilde{q}_i \right\|^2 &= \tilde{p}_i^\top R^\top R \tilde{p}_i - 2 \tilde{q}_i^\top R \tilde{p}_i + \tilde{q}_i^\top \tilde{q}_i \\ &= \left\| \tilde{p}_i \right\|^2 + \left\| \tilde{q}_i \right\|^2 - 2 \tilde{q}_i^\top R \tilde{p}_i. \end{aligned} \]

Summing over \( i \) and using \( \sum_i \tilde{q}_i^\top R \tilde{p}_i = \mathrm{tr}(R H) \), we obtain

\[ J(R,t^* ) = \sum_{i=1}^N \left( \left\| \tilde{p}_i \right\|^2 + \left\| \tilde{q}_i \right\|^2 \right) - 2\,\mathrm{tr}(R H) = \text{const} - 2\,\mathrm{tr}(R H). \]

Thus minimizing \( J(R,t^* ) \) is equivalent to maximizing \( \mathrm{tr}(R H) \).

Problem 3 (Monotonic Decrease of ICP Cost): Assume that (i) the correspondence step always chooses the closest point in Euclidean distance and (ii) the registration step computes the exact SVD-based optimum for those correspondences. Prove that the ICP cost \( \sum_i \left\| R^{(k)} p_i + t^{(k)} - q_{\pi^{(k)}(i)} \right\|^2 \) is nonincreasing with iterations.

Solution:

At iteration \( k \), let \( (R^{(k)}, t^{(k)}) \) be the current pose and \( \pi^{(k)} \) the correspondence mapping after the nearest-neighbor step. By definition of nearest neighbors,

\[ \sum_i \left\| R^{(k)} p_i + t^{(k)} - q_{\pi^{(k)}(i)} \right\|^2 \le \sum_i \left\| R^{(k)} p_i + t^{(k)} - q_{\pi^{(k-1)}(i)} \right\|^2, \]

since each \( q_{\pi^{(k)}(i)} \) is the closest target point to \( R^{(k)} p_i + t^{(k)} \). In the registration step, we fix \( \pi^{(k)} \) and optimize over \( (R,t) \), so

\[ \sum_i \left\| R^{(k+1)} p_i + t^{(k+1)} - q_{\pi^{(k)}(i)} \right\|^2 \le \sum_i \left\| R^{(k)} p_i + t^{(k)} - q_{\pi^{(k)}(i)} \right\|^2. \]

Combining the two inequalities shows that the ICP cost at iteration \( k+1 \) is no larger than at iteration \( k \), proving monotonic decrease.

Problem 4 (Weighted Registration): Suppose each correspondence \( (p_i, q_i) \) is assigned a positive weight \( w_i > 0 \). Consider the weighted cost

\[ J_w(R,t) = \sum_{i=1}^N w_i \left\| R p_i + t - q_i \right\|^2. \]

Derive expressions for the weighted centroids and the weighted cross-covariance matrix such that the same SVD-based procedure yields the optimal \( (R^* , t^* ) \).

Solution:

Define the total weight \( W = \sum_{i=1}^N w_i \) and the weighted centroids

\[ \bar{p}_w = \frac{1}{W} \sum_{i=1}^N w_i p_i, \quad \bar{q}_w = \frac{1}{W} \sum_{i=1}^N w_i q_i. \]

Let \( \tilde{p}_i = p_i - \bar{p}_w \) and \( \tilde{q}_i = q_i - \bar{q}_w \). Then the weighted cost can be decomposed as

\[ J_w(R,t) = \text{const} - 2\,\mathrm{tr}(R H_w), \]

where

\[ H_w = \sum_{i=1}^N w_i \tilde{p}_i \tilde{q}_i^\top. \]

The optimal translation is

\[ t^* = \bar{q}_w - R^* \bar{p}_w, \]

and \( R^* \) is obtained by applying the same SVD-based rotation formula to \( H_w \). Thus, the standard rigid registration solution generalizes naturally to weighted correspondences.

In this lesson we formulated the 3D object pose estimation problem from point clouds as a least-squares rigid registration task, derived the closed-form SVD solution for the rotation and translation under known correspondences, and embedded this solution inside the ICP algorithm for joint correspondence and pose estimation. We discussed robust extensions and a probabilistic interpretation that motivates more advanced models. Finally, we provided concrete implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, preparing the ground for integration of pose estimation into manipulation pipelines and for deeper perception topics in subsequent lessons.