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); // } }