C++ Quaterniond::normalize方法代码示例

IGL_INLINE void igl::two_axis_valuator_fixed_up(
  const int w,
  const int h,
  const double speed,
  const Eigen::Quaterniond & down_quat,
  const int down_x,
  const int down_y,
  const int mouse_x,
  const int mouse_y,
  Eigen::Quaterniond & quat)
{
  using namespace Eigen;
  Vector3d axis(0,1,0);
  quat = down_quat * 
    Quaterniond(
      AngleAxisd(
        PI*((double)(mouse_x-down_x))/(double)w*speed/2.0,
        axis.normalized()));
  quat.normalize();
  {
    Vector3d axis(1,0,0);
    if(axis.norm() != 0)
    {
      quat = 
        Quaterniond(
          AngleAxisd(
            PI*(mouse_y-down_y)/(double)h*speed/2.0,
            axis.normalized())) * quat;
      quat.normalize();
    }
  }
}

int main (int argc, char **argv)
{
  

 // Here's all the input***********************************************************************************************************
  
                              //doesn't load this flipped version of the file (reversing coordinates makes negative dot product calculations)
  vector<string> filenames = {wine_glass, paper_bowl, red_mug};
  vector<string> modelnames = {"wine_glass", "paper_bowl", "red_mug"};
  
  //make affine3d's
  Eigen::Quaterniond q;  
  q = Eigen::Quaterniond(0.5, 0.5, 0, 0);  //identity matrix
  q.normalize();
  Eigen::Affine3d aq = Eigen::Affine3d(q);
  Eigen::Affine3d t(Eigen::Translation3d(Eigen::Vector3d(-4,0,1.25)));  //wine_glass
  Eigen::Affine3d a = (t*aq); 
    
  q = Eigen::Quaterniond(0.5, 0.5, 0, 0);  //identity matrix
  q.normalize();
  aq = Eigen::Affine3d(q);
  t = (Eigen::Translation3d(Eigen::Vector3d(0,0,1.1)));  //paper bowl
  Eigen::Affine3d b = (t*aq); 
    
  q = Eigen::Quaterniond(0.5, 0.5, 0, 0);  //identity matrix
  q.normalize();
  aq = Eigen::Affine3d(q);
  t =  (Eigen::Translation3d(Eigen::Vector3d(4,0,0.66)));  //red mug
  Eigen::Affine3d c = (t*aq); 

  vector<Eigen::Affine3d> model_poses = {a,b,c};  //vector 1 of affines

  q = Eigen::Quaterniond(0.3, 0.7, 0, 0);  //NOT identity matrix
  q.normalize();
  aq = Eigen::Affine3d(q);
  t =  (Eigen::Translation3d(Eigen::Vector3d(-2,0, 1.59)));  //wine_glass
  Eigen::Affine3d a2 = (t*aq); 
  
  vector<Eigen::Affine3d> model_poses2 = {a2,b,c};  //vector of new affines
// Here's all the input^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^


  
  // construct object and set parameters
  SceneValidator *scene = new SceneValidator;    //construct the object
  scene->setParams("DRAW", true);                //visualize what's actually happening 
  scene->setParams("PRINT_CHKR_RSLT", true);     //print out the result of each check
  scene->setParams("STEP1", 1000);               //make the first check take 1000 steps so user has plenty of time to see what's going on
  scene->setScale(1, 200);                       //set modelnames[1] to be scaled down by factor of 200   

  // API functions
  scene->setModels(modelnames, filenames);       //set all the models and get their data  
  scene->isValidScene(modelnames, model_poses);  //check scene 1
  //Press Ctrl-X when window pops up to exit above^ and start the simulation below 
  scene->isValidScene(modelnames, model_poses2); //check scene 2

  
  return 0;
}

bool planning_models::quatFromMsg(const geometry_msgs::Quaternion &qmsg, Eigen::Quaterniond &q)
{
  q = Eigen::Quaterniond(qmsg.w, qmsg.x, qmsg.y, qmsg.z);
  double error = fabs(q.squaredNorm() - 1.0);
  if (error > 0.05)
  {
    ROS_ERROR("Quaternion is NOWHERE CLOSE TO NORMALIZED [x,y,z,w], [%.2f, %.2f, %.2f, %.2f]. Can't do much, returning identity.",
        qmsg.x, qmsg.y, qmsg.z, qmsg.w);
    q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
    return false;
  }
  else if(error > 1e-3)
    q.normalize();

  return true;
}

TEST (PCL, WarpPointRigid6DDouble)
{
  WarpPointRigid6D<PointXYZ, PointXYZ, double> warp;
  Eigen::Quaterniond q (0.4455, 0.9217, 0.3382, 0.3656);
  q.normalize ();
  Eigen::Vector3d t (0.82550, 0.11697, 0.44864);

  Eigen::VectorXd p (6);
  p[0] = t.x (); p[1] = t.y (); p[2] = t.z (); p[3] = q.x (); p[4] = q.y (); p[5] = q.z ();
  warp.setParam (p);

  PointXYZ pin (1, 2, 3), pout;
  warp.warpPoint (pin, pout);
  EXPECT_NEAR (pout.x, 4.15963, 1e-5);
  EXPECT_NEAR (pout.y, -1.51363, 1e-5);
  EXPECT_NEAR (pout.z, 0.922648, 1e-5);
}

void scale_quaternion(double r,Eigen::Quaterniond q,Eigen::Quaterniond &q_out){
  if (q.w() ==1){
    // BUG: 15dec2011
    // TODO: check if this is a rigerous solution
    // q.w() ==1 mean no translation. so just return the input
    // if q.w()==1 and r=0.5 ... what is created below will have w=0 ... invalid
    //std::cout << "unit quaternion input:\n";
    q_out = q;
    return;
  }
  double theta = acos(q.w());
  //double sin_theta_inv = 1/ sin(theta);
  q_out.w() = sin((1-r)*theta)  + sin(r*theta) * q.w();
  q_out.x() = sin(r*theta) * q.x();
  q_out.y() = sin(r*theta) * q.y();
  q_out.z() = sin(r*theta) * q.z();
  q_out.normalize();
}

// update the camera state given a new camera pose
void MotionModel::UpdateCameraPose(const cv::Point3d& newPosition, const cv::Vec4d& newOrientation)
{
  // Compute linear velocity
  cv::Vec3d newLinearVelocity( newPosition - position_ );
  // In order to be robust against fast camera movements linear velocity is smoothed over time
  newLinearVelocity = (newLinearVelocity + linearVelocity_) * 0.5;

  // compute rotation between q1 and q2: q2 * qInverse(q1);
  Eigen::Quaterniond newAngularVelocity = cv2eigen( newOrientation ) * orientation_.inverse();

  // In order to be robust against fast camera movements angular velocity is smoothed over time
  newAngularVelocity = newAngularVelocity.slerp(0.5, angularVelocity_);

  newAngularVelocity.normalize();

  // Update the current state variables

  position_ = newPosition;
  orientation_ = cv2eigen( newOrientation );
  linearVelocity_ = newLinearVelocity;
  angularVelocity_ = newAngularVelocity;
}

TEST(TFEigenConversions, eigen_tf_transform)
{
  tf::Transform t;
  Eigen::Affine3d k;
  Eigen::Quaterniond kq;
  kq.coeffs()(0) = gen_rand(-1.0,1.0);
  kq.coeffs()(1) = gen_rand(-1.0,1.0);
  kq.coeffs()(2) = gen_rand(-1.0,1.0);
  kq.coeffs()(3) = gen_rand(-1.0,1.0);
  kq.normalize();
  k.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
  k.rotate(kq);

  transformEigenToTF(k,t);
  for(int i=0; i < 3; i++)
  {
    ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
    for(int j=0; j < 3; j++)
    {      
      ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
    }
  }
}

Eigen::Matrix4d state2Matrix(state s) {
	// take a state and return the equivalent 4x4 homogeneous transformation matrix
	Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
	// rotation part
	Eigen::Quaterniond q;
	q.x() = s.qx;
	q.y() = s.qy;
	q.z() = s.qz;
	q.w() = s.qw;
	q.normalize();
	Eigen::Matrix3d rot = q.toRotationMatrix();
	mat.block<3, 3>(0, 0) = rot;
	// translation part
	Eigen::Vector3d t;
	t[0] = s.x;
	t[1] = s.y;
	t[2] = s.z;
	mat.block<3, 1>(0, 3) = t;
	// homogeneous
	mat(3, 3) = 1;
	return mat;

}

void CableDraw::DrawCylinder(Eigen::Vector3d a, Eigen::Vector3d b, double length)
{
    Eigen::Vector3d axis_vector = b - a;
    Eigen::Vector3d axis_vector_normalized = axis_vector.normalized();

    Eigen::Vector3d up_vector(0.0, 0.0, 1.0);
    Eigen::Vector3d right_axis_vector = axis_vector_normalized.cross(up_vector);
    right_axis_vector.normalize();

    double theta = axis_vector_normalized.dot(up_vector);
    double angle_rotation = -acos(theta);

    tf::Vector3 tf_right_axis_vector;
    tf::vectorEigenToTF(right_axis_vector, tf_right_axis_vector);

    // Create quaternion
    tf::Quaternion tf_q(tf_right_axis_vector, angle_rotation);

    // Convert back to Eigen
    Eigen::Quaterniond q;
    tf::quaternionTFToEigen(tf_q, q);

    // Rotate so that vector points along line
    q.normalize();

    // Draw marker
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = frame_id_;
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = visualization_msgs::Marker::CYLINDER;

    // Set the marker action.  Options are ADD and DELETE
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = (a[0] + b[0])/2.0;
    marker.pose.position.y = (a[1] + b[1])/2.0;
    marker.pose.position.z = (a[2] + b[2])/2.0;
    marker.pose.orientation.x = q.x();
    marker.pose.orientation.y = q.y();
    marker.pose.orientation.z = q.z();
    marker.pose.orientation.w = q.w();

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 2*radius_threshold_;
    marker.scale.y = 2*radius_threshold_;
    marker.scale.z = length;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 231.0/255.0;
    marker.color.g = 79.0/255.0;
    marker.color.b = 81.0/255.0;
    marker.color.a = 0.25;

    marker.lifetime = ros::Duration();

    // Publish the marker
    remove_area_pub_.publish(marker);
}

int main (int argc, char **argv)
{
  

 // Here's all the input***********************************************************************************************************
  
 /*   Doesn't load flipped version of file (reversing coordinate order in faces makes negative dot product calculations)
   Doing the flipping operation in some other software screws up center of mass calculations because 
   the cross product produced is negative.  Just unflip the flipped figures in software (meshlab), and in the future don't flip
   them at all.  Or at the very least, just make sure your rotation/translation of the model in some other
   software doesn't screw up the vertice order for a face. */

  vector<string> filenames = {filename};
  vector<string> modelnames = {"test_object"};
  
  //make affine3d's
  Eigen::Quaterniond q;  
  q = Eigen::Quaterniond(0.5, 0.5, 0, 0);   //identity matrix
  q.normalize();
  Eigen::Affine3d aq = Eigen::Affine3d(q);
  Eigen::Affine3d t(Eigen::Translation3d(Eigen::Vector3d(0,0,2)));  
  Eigen::Affine3d a = (t*aq); 
    
  q = Eigen::Quaterniond(0.7, 0.3, 0, 0);  //NOT identity matrix
  q.normalize();
  aq = Eigen::Affine3d(q);
  t = (Eigen::Translation3d(Eigen::Vector3d(0,0,2)));   
  Eigen::Affine3d b = (t*aq); 
    
  q = Eigen::Quaterniond(0.5, 0.5, 0, 0);   //identity matrix
  q.normalize();
  aq = Eigen::Affine3d(q);
  t =  (Eigen::Translation3d(Eigen::Vector3d(0,0,1)));
  Eigen::Affine3d c = (t*aq);   

  q = Eigen::Quaterniond(0.3, 0.7, 0, 0);   //NOT identity matrix
  q.normalize();
  aq = Eigen::Affine3d(q);
  t =  (Eigen::Translation3d(Eigen::Vector3d(0,0,1.7)));
  Eigen::Affine3d d = (t*aq); 
  
  vector<Eigen::Affine3d> model_poses1 = {a};
  vector<Eigen::Affine3d> model_poses2 = {b};
  vector<Eigen::Affine3d> model_poses3 = {c};
  vector<Eigen::Affine3d> model_poses4 = {d};  
// Here's all the input^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^


  // construct object and set parameters
  SceneValidator *scene = new SceneValidator;    //construct the object
  scene->setParams("DRAW", true);                //visualize what's actually happening 
  scene->setParams("STEP1", 1000);               //make the first check take 1000 steps (so viewer has plenty of time to see object's behavior)
  scene->setScale(0, 0.1);                       //set modelnames[0] to be scaled down by factor of 100   

  // API functions
  scene->setModels(modelnames, filenames);       //set model and get its data  
  scene->isValidScene(modelnames, model_poses1); //check scene 1
  scene->isValidScene(modelnames, model_poses2); //check scene 2
  scene->isValidScene(modelnames, model_poses3); //check scene 3
  scene->isValidScene(modelnames, model_poses4); //check scene 4


  return 0;
}

bool GazeboInterface::readJointStates()
{
	if(!ROSControlInterface::readJointStates())
		return false;

	if(m_last_modelStates)
	{
		std::vector<std::string>::const_iterator it = std::find(
			m_last_modelStates->name.begin(), m_last_modelStates->name.end(),
			m_modelName
		);

		if(it != m_last_modelStates->name.end())
		{
			int idx = it - m_last_modelStates->name.begin();

			//
			// Orientation feedback
			//

			// Retrieve the robot pose
			const geometry_msgs::Pose& pose = m_last_modelStates->pose[idx];

			// Set the robot orientation
			Eigen::Quaterniond quat;
			tf::quaternionMsgToEigen(pose.orientation, quat);
			quat.normalize();
			m_model->setRobotOrientation(quat);

			// Provide /odom if wanted. Convention: /odom is on the floor.
			if(m_publishOdom && m_last_modelStatesStamp - m_initTime > ros::Duration(3.0))
			{
				tf::StampedTransform trans;
				trans.frame_id_ = "/odom";
				trans.child_frame_id_ = "/ego_rot";
				trans.stamp_ = m_last_modelStatesStamp;
				trans.setIdentity();

				tf::Vector3 translation;
				tf::pointMsgToTF(pose.position, translation);

				trans.setOrigin(translation);

				Eigen::Quaterniond rot;
				rot = Eigen::AngleAxisd(m_model->robotEYaw(), Eigen::Vector3d::UnitZ());

				tf::Quaternion quat;
				tf::quaternionEigenToTF(rot, quat);

				trans.setRotation(quat);

				ROS_DEBUG("robot pos: Z = %f, yaw: %f, stamp: %f", translation.z(), m_model->robotEYaw(), trans.stamp_.toSec());
				ROS_DEBUG("robot pos tf: %f %f %f %f %f %f %f",
					trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z(),
					trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()
				);

				m_pub_tf.sendTransform(trans);
			}

			//
			// Angular velocity feedback
			//

			// Retrieve the robot twist
			const geometry_msgs::Twist& twist = m_last_modelStates->twist[idx];

			// Retrieve the robot angular velocity in global coordinates
			Eigen::Vector3d globalAngularVelocity;
			tf::vectorMsgToEigen(twist.angular, globalAngularVelocity);

			// Set the measured angular velocity (local coordinates)
			m_model->setRobotAngularVelocity(quat.conjugate() * globalAngularVelocity);

			//
			// Acceleration feedback
			//

			// We'd need an extra Gazebo plugin for acceleration sensing.
			// For now we can do without and simply calculate the current
			// acceleration due to gravity (neglect inertial accelerations).
			Eigen::Vector3d globalGravityAcceleration(0.0, 0.0, 9.81);

			// Set the measured acceleration (local coordinates)
			m_model->setAccelerationVector(quat.conjugate() * globalGravityAcceleration);

			//
			// Magnetic field vector feedback
			//

			// We assume that north is positive X. The values of 0.20G (horiz)
			// and 0.44G (vert) are approximately valid for central europe.
			Eigen::Vector3d globalMagneticVector(0.20, 0.00, -0.44); // In gauss

			// Set the measured magnetic field vector (local coordinates)
			m_model->setMagneticFieldVector(quat.conjugate() * globalMagneticVector);
		}
	}

	// Return success
//.........这里部分代码省略.........