Пример #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);

}
Пример #2
0
Eigen::Matrix< Tp, 4, 4 > transformationMatrix( Eigen::Quaternion< Tp > &quaternion, Eigen::Matrix< Tp, 3, 1 > &translation)
{
    Eigen::Matrix< Tp, 3, 3 > rr = quaternion.toRotationMatrix();
    Eigen::Matrix< Tp, 4, 4 > ttmm;
    ttmm << rr, translation, 0.0, 0.0, 0.0, 1.0;
    return ttmm;
}
Пример #3
0
 void transformF2W(Eigen::Matrix<double,3,4> &m, 
                   const Eigen::Matrix<double,4,1> &trans, 
                   const Eigen::Quaternion<double> &qrot)
 {
   m.block<3,3>(0,0) = qrot.toRotationMatrix();
   m.col(3) = trans.head(3);
 };
Пример #4
0
 // transforms
 void transformW2F(Eigen::Matrix<double,3,4> &m, 
                   const Eigen::Matrix<double,4,1> &trans, 
                   const Eigen::Quaternion<double> &qrot)
 {
   m.block<3,3>(0,0) = qrot.toRotationMatrix();//.transpose();
   m.col(3).setZero();         // make sure there's no translation
   m.col(3) = -m*trans;
 };
Пример #5
0
int main(int argc, char* argv[])
{
	ros::init(argc,argv,"navsts2odom");
	ros::NodeHandle nh, ph("~");

	//Get rotation between the two
	std::vector<double> rpy(3,0);
	ph.param("rpy",rpy,rpy);

	//Setup the LTP to Odom frame
	Eigen::Quaternion<double> q;
	labust::tools::quaternionFromEulerZYX(rpy[0], rpy[1], rpy[2],q);
	Eigen::Matrix3d rot = q.toRotationMatrix().transpose();

	ros::Publisher odom = nh.advertise<nav_msgs::Odometry>("uwsim_hook", 1);
	ros::Subscriber navsts = nh.subscribe<auv_msgs::NavSts>("navsts",1,
			boost::bind(&onNavSts, boost::ref(odom), boost::ref(rot), _1));

	ros::spin();
	return 0;
}
Пример #6
0
  OBAPI void SetTorsion(double *c, unsigned int ref[4], double setang, std::vector<int> atoms)
  {
    unsigned int cidx[4];
    cidx[0] = (ref[0] - 1) * 3;
    cidx[1] = (ref[1] - 1) * 3;
    cidx[2] = (ref[2] - 1) * 3;
    cidx[3] = (ref[3] - 1) * 3;
    const Eigen::Vector3d va( c[cidx[0]], c[cidx[0]+1], c[cidx[0]+2] );
    const Eigen::Vector3d vb( c[cidx[1]], c[cidx[1]+1], c[cidx[1]+2] );
    const Eigen::Vector3d vc( c[cidx[2]], c[cidx[2]+1], c[cidx[2]+2] );
    const Eigen::Vector3d vd( c[cidx[3]], c[cidx[3]+1], c[cidx[3]+2] );
    // calculate the rotation angle 
    const double ang = setang - DEG_TO_RAD * VectorTorsion(va, vb, vc, vd);
     
    // the angles are the same... 
    if (fabs(ang) < 1e-5)
      return;
    
    // setup the rotation matrix
    const Eigen::Vector3d bc = (vb - vc).normalized();
    Eigen::Quaternion<double> m;
    m = Eigen::AngleAxis<double>(-ang, bc);

    // apply the rotation
    int j;
    for (int i = 0; i < atoms.size(); ++i) {
      j = (atoms[i] - 1) * 3;
        
      Eigen::Vector3d vj( c[j], c[j+1], c[j+2] );
      vj -= vb; // translate so b is at origin
      vj = m.toRotationMatrix() * vj;
      vj += vb; // translate back 

      c[j]   = vj.x();
      c[j+1] = vj.y();
      c[j+2] = vj.z();
    }
  }