update

.gitignore

@@ -0,0 +1 @@
+build

build.bash

@@ -0,0 +1,27 @@
+#!/bin/bash
+
+# Check if the argument is provided
+if [ -z "$1" ]; then
+  echo "No C++ file provided."
+  exit 1
+fi
+
+# Extract the filename without extension
+filename=$(basename -- "$1")
+filename="${filename%.*}"
+
+mkdir -p "build/$filename"
+
+# Compile the C++ file
+g++ -o "build/$filename/$filename" "$1"
+
+# Check if the compilation was successful
+if [ $? -eq 0 ]; then
+  echo "Compilation successful. Executable is located at build/$filename/$filename"
+else
+  echo "Compilation failed."
+  exit 1
+fi
+
+echo "Running: $filename"
+build/$filename/$filename

umeyama.cpp

@@ -0,0 +1,45 @@
+#include <iostream>
+#include <Eigen/Dense>
+#include <iostream>
+using std::cout;
+using std::endl;
+
+int main() {
+    // Generate 20 random 2D points (source points)
+    Eigen::MatrixXd src_points(2, 20);
+    src_points = Eigen::MatrixXd::Random(2, 20);
+
+    // Define a known rotation matrix R and translation vector t
+    double theta = M_PI / 4;  // 45 degrees rotation
+    Eigen::Matrix2d R;
+    R << std::cos(theta), -std::sin(theta),
+         std::sin(theta),  std::cos(theta);
+    Eigen::Vector2d t(1.0, 2.0);
+
+    // Apply the transformation to generate the destination points
+    Eigen::MatrixXd dst_points = (R * src_points).colwise() + t;
+
+    // Use Eigen's Umeyama function to estimate the transformation
+    Eigen::Matrix3d T = Eigen::umeyama(src_points, dst_points, true);
+
+    // Print the estimated transformation matrix
+    std::cout << "Estimated transformation matrix:\n" << T << std::endl;
+
+    // Apply the resulting transformation to the source points
+    Eigen::MatrixXd src_points_hom(3, 20);
+    src_points_hom.topRows(2) = src_points;
+    src_points_hom.row(2) = Eigen::RowVectorXd::Ones(20);
+
+    Eigen::MatrixXd aligned_points = (T * src_points_hom).topRows(2);
+
+    // Print the original, transformed, and recovered points
+    std::cout << "Original Source Points:\n" << src_points << std::endl;
+    std::cout << "Transformed Destination Points:\n" << dst_points << std::endl;
+    std::cout << "Recovered Aligned Points:\n" << aligned_points << std::endl;
+
+    // Calculate the difference between the destination points and the aligned points
+    double difference = (dst_points - aligned_points).norm();
+    std::cout << "\nDifference between destination and aligned points: " << difference << std::endl;
+
+    return 0;
+}