78 lines
3.0 KiB
C++
78 lines
3.0 KiB
C++
#ifndef HANDEYECALIB_H
|
|
#define HANDEYECALIB_H
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#include "IHandEyeCalib.h"
|
|
|
|
class HandEyeCalib : public IHandEyeCalib
|
|
{
|
|
public:
|
|
HandEyeCalib();
|
|
~HandEyeCalib() override;
|
|
|
|
int CalculateRT(const std::vector<HECPoint3D>& eyePoints,
|
|
const std::vector<HECPoint3D>& robotPoints,
|
|
HECCalibResult& result) override;
|
|
|
|
int CalculateRT(const std::vector<HECCalibPointPair>& pointPairs,
|
|
HECCalibResult& result) override;
|
|
|
|
void TransformPoint(const HECRotationMatrix& R,
|
|
const HECTranslationVector& T,
|
|
const HECPoint3D& srcPoint,
|
|
HECPoint3D& dstPoint) override;
|
|
|
|
void RotatePoint(const HECRotationMatrix& R,
|
|
const HECPoint3D& srcPoint,
|
|
HECPoint3D& dstPoint) override;
|
|
|
|
void RotationMatrixToEuler(const HECRotationMatrix& R,
|
|
HECEulerOrder order,
|
|
HECEulerAngles& angles) override;
|
|
|
|
void EulerToRotationMatrix(const HECEulerAngles& angles,
|
|
HECEulerOrder order,
|
|
HECRotationMatrix& R) override;
|
|
|
|
bool TransformPose(const HECCalibResult& calibResult,
|
|
const HECPoint3D& eyeCenter,
|
|
const HECPoint3D& longAxisDir,
|
|
const HECPoint3D& normalDir,
|
|
int dirVectorInvert,
|
|
HECEulerOrder eulerOrder,
|
|
HECLongAxisDir longAxisMapping,
|
|
HECPoseResult& poseResult) override;
|
|
|
|
double CalculateError(const std::vector<HECPoint3D>& eyePoints,
|
|
const std::vector<HECPoint3D>& robotPoints,
|
|
const HECCalibResult& calibResult) override;
|
|
|
|
int CalculateEyeInHand(const std::vector<HECEyeInHandData>& calibData,
|
|
HECCalibResult& result) override;
|
|
|
|
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,
|
|
HECEulerOrder order);
|
|
};
|
|
|
|
#endif // HANDEYECALIB_H
|