diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/build.bash b/build.bash new file mode 100755 index 0000000..0fcb130 --- /dev/null +++ b/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 diff --git a/umeyama.cpp b/umeyama.cpp new file mode 100644 index 0000000..cb87185 --- /dev/null +++ b/umeyama.cpp @@ -0,0 +1,45 @@ +#include +#include +#include +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; +}