Esempio n. 1
2
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);
//    }
    }