46 lines
1.7 KiB
C++
46 lines
1.7 KiB
C++
|
#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;
|
||
|
|
}
|