PlaneExt::PlaneExt(but_plane_detector::Plane<float> plane) : but_plane_detector::Plane<float> (0.0, 0.0, 0.0, 0.0), planeCoefficients(new pcl::ModelCoefficients())
{
a = plane.a;
b = plane.b;
c = plane.c;
d = plane.d;
norm = plane.norm;
// Create a quaternion for rotation into XY plane
Eigen::Vector3f current(a, b, c);
Eigen::Vector3f target(0.0, 0.0, 1.0);
Eigen::Quaternion<float> q;
q.setFromTwoVectors(current, target);
planeTransXY = q.toRotationMatrix();
planeShift = -d;
color.r = abs(a) / 2.0 + 0.2;
color.g = abs(b) / 2.0 + 0.2;
color.b = abs(d) / 5.0;
color.a = 1.0;
// Plane coefficients pre-calculation...
planeCoefficients->values.push_back(a);
planeCoefficients->values.push_back(b);
planeCoefficients->values.push_back(c);
planeCoefficients->values.push_back(d);
}
inline static void setFromVectors(Rotation_& rot, const Eigen::Matrix<PrimType_, 3, 1>& v1, const Eigen::Matrix<PrimType_, 3, 1>& v2) {
KINDR_ASSERT_TRUE(std::runtime_error, v1.norm()*v2.norm() != static_cast<PrimType_>(0.0), "At least one vector has zero length.");
Eigen::Quaternion<PrimType_> eigenQuat;
eigenQuat.setFromTwoVectors(v1, v2);
rot = kindr::rotations::eigen_impl::RotationQuaternion<PrimType_, Usage_>(eigenQuat);
//
// const PrimType_ angle = acos(v1.dot(v2)/temp);
// const PrimType_ tol = 1e-3;
//
// if(0 <= angle && angle < tol) {
// rot.setIdentity();
// } else if(M_PI - tol < angle && angle < M_PI + tol) {
// rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, 1, 0, 0);
// } else {
// const Eigen::Matrix<PrimType_, 3, 1> axis = (v1.cross(v2)).normalized();
// rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, axis);
// }
}