123 lines
5.7 KiB
C++
123 lines
5.7 KiB
C++
#include <opencv.hpp>
|
||
#include <Eigen/Dense>
|
||
using namespace cv;
|
||
using namespace std;
|
||
Eigen::Vector3d calculateCentroid(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2, const Eigen::Vector3d& P3) {
|
||
return (P1 + P2 + P3) / 3.0;
|
||
}
|
||
Eigen::Vector3d calculateTranslation(const Eigen::Vector3d& A1, const Eigen::Vector3d& A2, const Eigen::Vector3d& A3,
|
||
const Eigen::Vector3d& B1, const Eigen::Vector3d& B2, const Eigen::Vector3d& B3) {
|
||
Eigen::Vector3d centroidA = calculateCentroid(A1, A2, A3);
|
||
Eigen::Vector3d centroidB = calculateCentroid(B1, B2, B3);
|
||
return centroidB - centroidA;
|
||
}
|
||
Eigen::Matrix3d calculateRotationMatrix(const Eigen::Vector3d& A1, const Eigen::Vector3d& A2, const Eigen::Vector3d& A3,
|
||
const Eigen::Vector3d& B1, const Eigen::Vector3d& B2, const Eigen::Vector3d& B3) {
|
||
// 定义物体 A 的局部坐标系
|
||
Eigen::Vector3d v1_A = (A2 - A1).normalized();
|
||
Eigen::Vector3d v2_A = (A3 - A1).normalized();
|
||
Eigen::Vector3d v3_A = v1_A.cross(v2_A).normalized();
|
||
Eigen::Matrix3d RA;
|
||
RA.col(0) = v1_A;
|
||
RA.col(1) = v3_A.cross(v1_A).normalized();
|
||
RA.col(2) = v3_A;
|
||
|
||
// 定义物体 B 的局部坐标系
|
||
Eigen::Vector3d v1_B = (B2 - B1).normalized();
|
||
Eigen::Vector3d v2_B = (B3 - B1).normalized();
|
||
Eigen::Vector3d v3_B = v1_B.cross(v2_B).normalized();
|
||
Eigen::Matrix3d RB;
|
||
RB.col(0) = v1_B;
|
||
RB.col(1) = v3_B.cross(v1_B).normalized();
|
||
RB.col(2) = v3_B;
|
||
|
||
// 计算旋转矩阵
|
||
Eigen::Matrix3d R_AB = RB * RA.transpose();
|
||
return R_AB;
|
||
}
|
||
|
||
Eigen::Vector3d calculateEulerAngles(const Eigen::Matrix3d& R) {
|
||
return R.eulerAngles(0, 1, 2); // 返回绕 X, Y, Z 轴的欧拉角（Yaw-Pitch-Roll）
|
||
}
|
||
Eigen::Vector3d calculateCentroid(const std::vector<Eigen::Vector3d>& points) {
|
||
Eigen::Vector3d centroid(0, 0, 0);
|
||
for (const auto& point : points) {
|
||
centroid += point;
|
||
}
|
||
centroid /= points.size();
|
||
return centroid;
|
||
}
|
||
Eigen::Matrix3d calculateRotationMatrixSVD(const std::vector<Eigen::Vector3d>& A_points, const std::vector<Eigen::Vector3d>& B_points) {
|
||
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
|
||
for (size_t i = 0; i < A_points.size(); ++i) {
|
||
H += (B_points[i] - calculateCentroid(B_points)) * (A_points[i] - calculateCentroid(A_points)).transpose();
|
||
}
|
||
Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||
Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();
|
||
return R;
|
||
}
|
||
|
||
//std::vector<Eigen::Vector3d> P1 = {
|
||
// Eigen::Vector3d(453.64, -1373.59, -203.9841), // -130.33 - 73.6541
|
||
// Eigen::Vector3d(462.86, -1228.2 , -206.8113), // -130.42 - 76.3913
|
||
// Eigen::Vector3d(723.69, -1228.2 , -206.3582), // -130.43 - 75.9282
|
||
// Eigen::Vector3d(755.77, -447.5516, -967.41), // -385.56 - 61.9916
|
||
// Eigen::Vector3d(756.7 , -447.9429, -999.2), // -385.56 - 62.3829
|
||
// Eigen::Vector3d(347.92, -450.0665, -1000.87), // -385.48 - 64.5865
|
||
// Eigen::Vector3d(-120.6791, -1024.55, -599.13), // -132.24 + 11.5609
|
||
// Eigen::Vector3d(-120.9298, -1024.53, -680.57), // -132.24 + 11.3102
|
||
// Eigen::Vector3d(-121.3208, -1098.14, -680.15) // -132.28 + 10.9592
|
||
//};
|
||
|
||
//std::vector<Eigen::Vector3d> P1 = {
|
||
// Eigen::Vector3d(453.65, -1373.59, -203.9069), // -130.33 - 73.5769
|
||
// Eigen::Vector3d(462.86, -1228.2 , -206.8159), // -130.42 - 76.3959
|
||
// Eigen::Vector3d(723.65, -1228.18, -206.21), // -130.4 - 75.81
|
||
// Eigen::Vector3d(755.82, -481.713, -967.43), // -385.57 - 96.143
|
||
// Eigen::Vector3d(756.2 , -481.9407, -1000.99), // -385.56 - 96.3807
|
||
// Eigen::Vector3d(347.96, -484.8419, -1000.87), // -385.48 - 99.3619
|
||
// Eigen::Vector3d(-120.3304, -1024.55, -599.13), // -132.24 + 11.9096
|
||
// Eigen::Vector3d(-120.3962, -1024.52, -680.58), // -132.24 + 11.8438
|
||
// Eigen::Vector3d(-121.0291, -1098.13, -680.17) // -132.27 + 11.2409
|
||
//};
|
||
|
||
void oneArea()//:)
|
||
{
|
||
Eigen::Vector3d A1(755.77, -447.5516, -967.41), A2(756.7, -447.9429, -999.2), A3(347.92, -450.0665, -1000.87);
|
||
Eigen::Vector3d B1(755.82, -481.713, -967.43), B2(756.2, -481.9407, -1000.99), B3(347.96, -484.8419, -1000.87);
|
||
|
||
// 计算偏移量
|
||
Eigen::Vector3d translation = calculateTranslation(A1, A2, A3, B1, B2, B3);
|
||
std::cout << "Translation: \n" << translation << std::endl;
|
||
|
||
Eigen::Matrix3d rotationMatrix = calculateRotationMatrix(A1, A2, A3, B1, B2, B3);
|
||
Eigen::Vector3d eulerAngles = calculateEulerAngles(rotationMatrix);
|
||
std::cout << "Euler Angles (Yaw, Pitch, Roll): \n" << eulerAngles << std::endl;
|
||
}
|
||
void threeArea()//:(
|
||
{
|
||
std::vector<Eigen::Vector3d> A_points = {
|
||
{453.64, -1373.59, -203.9841}, {462.86, -1228.2 , -206.8113}, {723.69, -1228.2 , -206.3582}, // 面 1
|
||
{755.77, -447.5516, -967.41}, {756.7 , -447.9429, -999.2}, {347.92, -450.0665, -1000.87}, // 面 2
|
||
{-120.6791, -1024.55, -599.13}, {-120.9298, -1024.53, -680.57}, {-121.3208, -1098.14, -680.15} // 面 3
|
||
};
|
||
|
||
std::vector<Eigen::Vector3d> B_points = {
|
||
{453.65, -1373.59, -203.9069}, {462.86, -1228.2 , -206.8159}, {347.96, -484.8419, -1000.87}, // 面 1
|
||
{755.82, -481.713, -967.43}, {756.2 , -481.9407, -1000.99}, {347.92, -450.0665, -1000.8}, // 面 2
|
||
{-120.3304, -1024.55, -599.13}, {-120.3962, -1024.52, -680.58}, {-121.0291, -1098.13, -680.17} // 面 3
|
||
};
|
||
|
||
// 计算质心（用于计算平移量）
|
||
Eigen::Vector3d centroidA = calculateCentroid(A_points);
|
||
Eigen::Vector3d centroidB = calculateCentroid(B_points);
|
||
Eigen::Vector3d translation = centroidB - centroidA;
|
||
std::cout << "Translation: \n" << translation << std::endl;
|
||
|
||
// 计算旋转矩阵和欧拉角
|
||
Eigen::Matrix3d R_AB = calculateRotationMatrixSVD(A_points, B_points);
|
||
Eigen::Vector3d eulerAngles = calculateEulerAngles(R_AB);
|
||
std::cout << "Euler Angles (Yaw, Pitch, Roll): \n" << eulerAngles << std::endl;
|
||
|
||
}
|
||
#pragma once |