feasibilitychecker.cpp

Go to the documentation of this file.

#include <MathHelpers/Resectionsolver/feasibilitychecker.h>
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif

FeasibilityChecker::FeasibilityChecker(boost::shared_ptr<Calibration_Object> calibrationObject, const Eigen::Vector3d &expectedOrientationz, double maximumAngleDeviation)
{
    this->calibrationObject = calibrationObject;
    this->expectedOrientation_z = Eigen::Vector4d(expectedOrientationz[0], expectedOrientationz[1], expectedOrientationz[2], 0.0);
    this->maximumAngleDeviation = maximumAngleDeviation;
}

bool FeasibilityChecker::checkFeasibility_sideLengths(double sideA, double sideB, double sideC)
{
    return (sideA <= calibrationObject->side_a && sideB <= calibrationObject->side_b && sideC <= calibrationObject->side_c);
}

bool FeasibilityChecker::checkFeasibility_pose(const Eigen::Matrix4d& pose)
{
    Eigen::Vector4d zAxis(0.0,0.0,1.0,0.0);
    zAxis = pose*zAxis;
    double angleDeviation = 1.0 - std::fabs(zAxis.dot(this->expectedOrientation_z));
    return angleDeviation < maximumAngleDeviation;
}