Go to the documentation of this file.00001
00017 #include <MathHelpers/Resectionsolver/feasibilitychecker.h>
00018 #ifndef Q_MOC_RUN
00019 #include <ros/ros.h>
00020 #endif
00021
00022 FeasibilityChecker::FeasibilityChecker(boost::shared_ptr<Calibration_Object> calibrationObject, const Eigen::Vector3d &expectedOrientationz, double maximumAngleDeviation)
00023 {
00024 this->calibrationObject = calibrationObject;
00025 this->expectedOrientation_z = Eigen::Vector4d(expectedOrientationz[0], expectedOrientationz[1], expectedOrientationz[2], 0.0);
00026 this->maximumAngleDeviation = maximumAngleDeviation;
00027 }
00028
00029 bool FeasibilityChecker::checkFeasibility_sideLengths(double sideA, double sideB, double sideC)
00030 {
00031 return (sideA <= calibrationObject->side_a && sideB <= calibrationObject->side_b && sideC <= calibrationObject->side_c);
00032 }
00033
00034 bool FeasibilityChecker::checkFeasibility_pose(const Eigen::Matrix4d& pose)
00035 {
00036 Eigen::Vector4d zAxis(0.0,0.0,1.0,0.0);
00037 zAxis = pose*zAxis;
00038 double angleDeviation = 1.0 - std::fabs(zAxis.dot(this->expectedOrientation_z));
00039 return angleDeviation < maximumAngleDeviation;
00040 }