工件孔定位算法姿态更新
This commit is contained in:
杰仔
parent
ad5a7f40b5
commit
1e1b373ddd
@ -5,8 +5,8 @@
|
||||
#define WORKPIECEHOLE_APP_NAME "工件孔定位"
|
||||
|
||||
// 版本字符串
|
||||
#define WORKPIECEHOLE_VERSION_STRING "1.1.3"
|
||||
#define WORKPIECEHOLE_BUILD_STRING "2"
|
||||
#define WORKPIECEHOLE_VERSION_STRING "1.1.4"
|
||||
#define WORKPIECEHOLE_BUILD_STRING "1"
|
||||
#define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING
|
||||
|
||||
// 构建日期
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -6,6 +6,20 @@
|
||||
|
||||
//向量叉乘
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b);
|
||||
// 向量数乘
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint vec3_multiply(const SVzNL3DPoint& a, const double s);
|
||||
// 点乘 dot
|
||||
SG_APISHARED_EXPORT double vec3_dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b);
|
||||
// 模长
|
||||
SG_APISHARED_EXPORT double vec3_length(const SVzNL3DPoint& a);
|
||||
// 归一化（单位向量）
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a);
|
||||
|
||||
/**
|
||||
* @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度
|
||||
* （v 必须在平面内，自动使用简化版罗德里格斯）
|
||||
*/
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint wd_rotateVectorInPlane(const SVzNL3DPoint& v, const SVzNL3DPoint& n, double theta);
|
||||
|
||||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle);
|
||||
@ -144,6 +158,14 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM(
|
||||
const SSG_cornerParam cornerPara,
|
||||
SSG_lineFeature* line_features);
|
||||
|
||||
//根据距离连续性分段
|
||||
SG_APISHARED_EXPORT void wd_lineDataSegment_dist(
|
||||
std::vector< SWD3DPointPostion>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
const double maxDistTh,
|
||||
const int minSegSize
|
||||
);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_lineDataSegment_zDist(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
@ -183,6 +205,16 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod(
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
);
|
||||
|
||||
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
|
||||
SG_APISHARED_EXPORT void wd_searchCornerPeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
const double minCornerTh,
|
||||
double cornerMergeScale,
|
||||
std::vector< SSG_pntDirAngle>& cornerPeakP,
|
||||
std::vector< SSG_pntDirAngle>& cornerPeakM
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
@ -538,6 +570,13 @@ SG_APISHARED_EXPORT void lineFitting_abc(
|
||||
double* _b,
|
||||
double* _c);
|
||||
|
||||
//拟合成通用直线方程ax+by+c=0，包括垂直
|
||||
SG_APISHARED_EXPORT void indexingPtLineFitting_abc(
|
||||
std::vector< SWD3DPointPostion>& inliers,
|
||||
double* _a,
|
||||
double* _b,
|
||||
double* _c);
|
||||
|
||||
/**
|
||||
* @brief 空间直线最小二乘拟合
|
||||
* @param points 输入的三维点集（至少2个点，否则无意义）
|
||||
|
||||
@ -39,6 +39,13 @@ typedef struct
|
||||
SWD3DPoint point;
|
||||
}SWDIndexing3DPoint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int lineIdx;
|
||||
int ptIdx;
|
||||
SVzNL3DPoint point;
|
||||
}SWD3DPointPostion;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int lineIdx;
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -56,6 +56,14 @@ public:
|
||||
virtual int CalculateEyeInHandWithPose(const std::vector<HECEyeInHandPoseData>& calibData,
|
||||
HECCalibResult& result) = 0;
|
||||
|
||||
// 将旋转矩阵安全地转换为指定顺序的欧拉角（带万向节锁处理和帧间连续性修正）
|
||||
// order: 目标欧拉角旋转顺序
|
||||
// prevAngles: 上一帧输出的角度指针，传入则启用帧间连续性修正，传nullptr则不修正
|
||||
virtual void RotationMatrixToEulerZYXSafe(const HECRotationMatrix& R,
|
||||
HECEulerOrder order,
|
||||
const HECEulerAngles* prevAngles,
|
||||
HECEulerAngles& angles) = 0;
|
||||
|
||||
virtual HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) = 0;
|
||||
|
||||
virtual int CalculateEyeToHandWithPose(const std::vector<HECEyeToHandData>& calibData,
|
||||
|
||||
@ -798,6 +798,209 @@ Eigen::Matrix3d HandEyeCalib::eulerToRotationMatrix(double rx, double ry, double
|
||||
return result;
|
||||
}
|
||||
|
||||
void HandEyeCalib::decomposeStandard(const Eigen::Matrix3d& Rm, HECEulerOrder order, HECEulerAngles& angles)
|
||||
{
|
||||
switch (order) {
|
||||
case HECEulerOrder::ZYX:
|
||||
// R = Rz(yaw) · Ry(pitch) · Rx(roll)
|
||||
angles.pitch = std::asin(-Rm(2, 0));
|
||||
angles.yaw = std::atan2(Rm(1, 0), Rm(0, 0));
|
||||
angles.roll = std::atan2(Rm(2, 1), Rm(2, 2));
|
||||
break;
|
||||
case HECEulerOrder::ZXY:
|
||||
// R = Rz(yaw) · Rx(roll) · Ry(pitch)
|
||||
angles.roll = std::asin(Rm(2, 1));
|
||||
angles.yaw = std::atan2(-Rm(0, 1), Rm(1, 1));
|
||||
angles.pitch = std::atan2(-Rm(2, 0), Rm(2, 2));
|
||||
break;
|
||||
case HECEulerOrder::XYZ:
|
||||
// R = Rx(roll) · Ry(pitch) · Rz(yaw)
|
||||
angles.pitch = std::asin(Rm(0, 2));
|
||||
angles.roll = std::atan2(-Rm(1, 2), Rm(2, 2));
|
||||
angles.yaw = std::atan2(-Rm(0, 1), Rm(0, 0));
|
||||
break;
|
||||
case HECEulerOrder::XZY:
|
||||
// R = Rx(roll) · Rz(yaw) · Ry(pitch)
|
||||
angles.yaw = std::asin(-Rm(0, 1));
|
||||
angles.roll = std::atan2(Rm(2, 1), Rm(1, 1));
|
||||
angles.pitch = std::atan2(Rm(0, 2), Rm(0, 0));
|
||||
break;
|
||||
case HECEulerOrder::YXZ:
|
||||
// R = Ry(pitch) · Rx(roll) · Rz(yaw)
|
||||
angles.roll = std::asin(-Rm(1, 2));
|
||||
angles.pitch = std::atan2(Rm(0, 2), Rm(2, 2));
|
||||
angles.yaw = std::atan2(Rm(1, 0), Rm(1, 1));
|
||||
break;
|
||||
case HECEulerOrder::YZX:
|
||||
// R = Ry(pitch) · Rz(yaw) · Rx(roll)
|
||||
angles.yaw = std::asin(Rm(1, 0));
|
||||
angles.pitch = std::atan2(-Rm(2, 0), Rm(0, 0));
|
||||
angles.roll = std::atan2(-Rm(1, 2), Rm(1, 1));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void HandEyeCalib::RotationMatrixToEulerZYXSafe(
|
||||
const HECRotationMatrix& R,
|
||||
HECEulerOrder order,
|
||||
const HECEulerAngles* prevAngles,
|
||||
HECEulerAngles& angles)
|
||||
{
|
||||
// Step 1: 将 HECRotationMatrix 转换为 Eigen::Matrix3d
|
||||
Eigen::Matrix3d Rm;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
Rm(i, j) = R.at(i, j);
|
||||
|
||||
constexpr double SINGULAR_THRESHOLD = 0.9998;
|
||||
|
||||
// Step 2: 根据 order 计算中间轴 sin 值，检测奇异区
|
||||
// 各顺序的 sing_val = sin(中间轴角度)，>0 对应 +90°，<0 对应 -90°
|
||||
double sing_val = 0;
|
||||
switch (order) {
|
||||
case HECEulerOrder::ZYX: sing_val = -Rm(2, 0); break; // Ry(pitch)
|
||||
case HECEulerOrder::ZXY: sing_val = Rm(2, 1); break; // Rx(roll)
|
||||
case HECEulerOrder::XYZ: sing_val = Rm(0, 2); break; // Ry(pitch)
|
||||
case HECEulerOrder::XZY: sing_val = -Rm(0, 1); break; // Rz(yaw)
|
||||
case HECEulerOrder::YXZ: sing_val = -Rm(1, 2); break; // Rx(roll)
|
||||
case HECEulerOrder::YZX: sing_val = Rm(1, 0); break; // Rz(yaw)
|
||||
}
|
||||
|
||||
if (std::abs(sing_val) > SINGULAR_THRESHOLD) {
|
||||
// === Steps 3-5: 绕路分解 ===
|
||||
// 绕路顺序：中间轴不同的配对顺序，奇异区不重叠
|
||||
// ZYX↔ZXY, XYZ↔XZY, YXZ↔YZX
|
||||
HECEulerOrder bypass;
|
||||
switch (order) {
|
||||
case HECEulerOrder::ZYX: bypass = HECEulerOrder::ZXY; break;
|
||||
case HECEulerOrder::ZXY: bypass = HECEulerOrder::ZYX; break;
|
||||
case HECEulerOrder::XYZ: bypass = HECEulerOrder::XZY; break;
|
||||
case HECEulerOrder::XZY: bypass = HECEulerOrder::XYZ; break;
|
||||
case HECEulerOrder::YXZ: bypass = HECEulerOrder::YZX; break;
|
||||
case HECEulerOrder::YZX: bypass = HECEulerOrder::YXZ; break;
|
||||
}
|
||||
|
||||
// Step 3: 用绕路顺序分解（绕路顺序在当前区域不奇异，数值稳定）
|
||||
HECEulerAngles bypassAngles;
|
||||
RotationMatrixToEuler(R, bypass, bypassAngles);
|
||||
|
||||
// Step 4: 用绕路结果重建 R'，验证与原始 R 的误差
|
||||
HECRotationMatrix Rp_hec;
|
||||
EulerToRotationMatrix(bypassAngles, bypass, Rp_hec);
|
||||
Eigen::Matrix3d Rp;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
Rp(i, j) = Rp_hec.at(i, j);
|
||||
|
||||
double err = (Rp - Rm).norm();
|
||||
|
||||
// Step 5: 对 R' 按目标顺序的万向锁公式分解（设定末轴为0）
|
||||
if (err < 1e-6) {
|
||||
switch (order) {
|
||||
case HECEulerOrder::ZYX:
|
||||
if (sing_val > 0) {
|
||||
// pitch ≈ +90°
|
||||
angles.pitch = M_PI / 2.0;
|
||||
angles.roll = 0.0;
|
||||
angles.yaw = -std::atan2(Rp(0, 1), Rp(1, 1));
|
||||
} else {
|
||||
angles.pitch = -M_PI / 2.0;
|
||||
angles.roll = 0.0;
|
||||
angles.yaw = std::atan2(-Rp(0, 1), Rp(1, 1));
|
||||
}
|
||||
break;
|
||||
|
||||
case HECEulerOrder::ZXY:
|
||||
if (sing_val > 0) {
|
||||
angles.roll = M_PI / 2.0;
|
||||
angles.pitch = 0.0;
|
||||
angles.yaw = std::atan2(Rp(1, 0), Rp(0, 0));
|
||||
} else {
|
||||
angles.roll = -M_PI / 2.0;
|
||||
angles.pitch = 0.0;
|
||||
angles.yaw = std::atan2(Rp(1, 0), Rp(0, 0));
|
||||
}
|
||||
break;
|
||||
|
||||
case HECEulerOrder::XYZ:
|
||||
if (sing_val > 0) {
|
||||
angles.pitch = M_PI / 2.0;
|
||||
angles.yaw = 0.0;
|
||||
angles.roll = std::atan2(Rp(1, 0), Rp(1, 1));
|
||||
} else {
|
||||
angles.pitch = -M_PI / 2.0;
|
||||
angles.yaw = 0.0;
|
||||
angles.roll = std::atan2(-Rp(1, 0), Rp(1, 1));
|
||||
}
|
||||
break;
|
||||
|
||||
case HECEulerOrder::XZY:
|
||||
if (sing_val > 0) {
|
||||
angles.yaw = M_PI / 2.0;
|
||||
angles.pitch = 0.0;
|
||||
angles.roll = std::atan2(Rp(2, 0), Rp(1, 0));
|
||||
} else {
|
||||
angles.yaw = -M_PI / 2.0;
|
||||
angles.pitch = 0.0;
|
||||
angles.roll = std::atan2(-Rp(2, 0), -Rp(1, 0));
|
||||
}
|
||||
break;
|
||||
|
||||
case HECEulerOrder::YXZ:
|
||||
if (sing_val > 0) {
|
||||
angles.roll = M_PI / 2.0;
|
||||
angles.yaw = 0.0;
|
||||
angles.pitch = std::atan2(Rp(0, 1), Rp(0, 0));
|
||||
} else {
|
||||
angles.roll = -M_PI / 2.0;
|
||||
angles.yaw = 0.0;
|
||||
angles.pitch = std::atan2(-Rp(0, 1), Rp(0, 0));
|
||||
}
|
||||
break;
|
||||
|
||||
case HECEulerOrder::YZX:
|
||||
if (sing_val > 0) {
|
||||
angles.yaw = M_PI / 2.0;
|
||||
angles.roll = 0.0;
|
||||
angles.pitch = std::atan2(Rp(2, 1), Rp(2, 2));
|
||||
} else {
|
||||
angles.yaw = -M_PI / 2.0;
|
||||
angles.roll = 0.0;
|
||||
angles.pitch = std::atan2(Rp(0, 2), Rp(0, 1));
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
double rollDeg, pitchDeg, yawDeg;
|
||||
angles.toDegrees(rollDeg, pitchDeg, yawDeg);
|
||||
LOG_INFO("[HandEyeCalib] 万向锁处理(%d): roll=%.2f°, pitch=%.2f°, yaw=%.2f°, R验证误差=%.2e\n",
|
||||
static_cast<int>(order), rollDeg, pitchDeg, yawDeg, err);
|
||||
} else {
|
||||
// 验证失败（不应发生），回退标准分解
|
||||
decomposeStandard(Rm, order, angles);
|
||||
LOG_INFO("[HandEyeCalib] 绕路验证失败(err=%.2e)，回退标准分解\n", err);
|
||||
}
|
||||
} else {
|
||||
// Step 6: 非奇异区直接按目标顺序分解
|
||||
decomposeStandard(Rm, order, angles);
|
||||
}
|
||||
|
||||
// Step 7: 帧间连续性修正（任意轴突变超过150°则加减360°）
|
||||
if (prevAngles) {
|
||||
constexpr double JUMP_THRESHOLD = 150.0 * M_PI / 180.0;
|
||||
auto correct = [](double& cur, double prev, double thr) {
|
||||
double delta = cur - prev;
|
||||
if (delta > thr) cur -= 2.0 * M_PI;
|
||||
else if (delta < -thr) cur += 2.0 * M_PI;
|
||||
};
|
||||
correct(angles.yaw, prevAngles->yaw, JUMP_THRESHOLD);
|
||||
correct(angles.pitch, prevAngles->pitch, JUMP_THRESHOLD);
|
||||
correct(angles.roll, prevAngles->roll, JUMP_THRESHOLD);
|
||||
}
|
||||
|
||||
// Step 8: 输出已完成（angles 包含 roll, pitch, yaw）
|
||||
}
|
||||
|
||||
HECTCPCalibResult HandEyeCalib::CalculateTCP(const HECTCPCalibData& data)
|
||||
{
|
||||
HECTCPCalibResult result;
|
||||
|
||||
@ -54,12 +54,20 @@ public:
|
||||
int CalculateEyeInHandWithPose(const std::vector<HECEyeInHandPoseData>& calibData,
|
||||
HECCalibResult& result) override;
|
||||
|
||||
void RotationMatrixToEulerZYXSafe(const HECRotationMatrix& R,
|
||||
HECEulerOrder order,
|
||||
const HECEulerAngles* prevAngles,
|
||||
HECEulerAngles& angles) override;
|
||||
|
||||
HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) override;
|
||||
|
||||
int CalculateEyeToHandWithPose(const std::vector<HECEyeToHandData>& calibData,
|
||||
HECCalibResult& result) override;
|
||||
|
||||
private:
|
||||
// 非奇异区的标准分解（各顺序的 atan2/asin 公式）
|
||||
void decomposeStandard(const Eigen::Matrix3d& Rm, HECEulerOrder order, HECEulerAngles& angles);
|
||||
|
||||
Eigen::Matrix3d eulerToRotationMatrix(double rx,
|
||||
double ry,
|
||||
double rz,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user