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

visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){

  //------- Compute Principal Componets for Marker publishing


  //Get the principal components and the quaternion
  Eigen::Matrix3f evecs;
  Eigen::Vector3f evals;
  //pcl::eigen33 (covariance_matrix, evecs, evals);
  Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
	
  evecs = es.eigenvectors().real();
  evals = es.eigenvalues().real();
	    
  Eigen::Matrix3f rotation;
  rotation.row (0) = evecs.col (0);
  rotation.row (1) = evecs.col (1);
  rotation.row (2) = rotation.row (0).cross (rotation.row (1));
	    
  rotation.transposeInPlace ();
  Eigen::Quaternion<float> qt (rotation);
  qt.normalize ();
	    
  //Publish Marker for cluster
  visualization_msgs::Marker marker;	
		
  marker.header.frame_id = base_frame_;
  marker.header.stamp = ros::Time().now();
  marker.ns = "Perception";
  marker.action = visualization_msgs::Marker::ADD;
  marker.id = marker_id;	
  marker.lifetime = ros::Duration(1);	
		
  //centroid position
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];	
  marker.pose.orientation.x = qt.x();
  marker.pose.orientation.y = qt.y();
  marker.pose.orientation.z = qt.z();
  marker.pose.orientation.w = qt.w();			

  //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;

  marker.scale.x = sqrt(evals(0)) * 4;
  marker.scale.y = sqrt(evals(1)) * 4;
  marker.scale.z = sqrt(evals(2)) * 4;
	

  //give it some color!
  marker.color.a = 0.75;
  satToRGB(marker.color.r, marker.color.g, marker.color.b);

  //std::cout << "marker being published" << std::endl;

  return marker;
}

 // returns the local R,t in nd0 that produces nd1
 // NOTE: returns a postfix rotation; should return a prefix
 void transformN2N(Eigen::Matrix<double,4,1> &trans, 
                   Eigen::Quaternion<double> &qr,
                   Node &nd0, Node &nd1)
 {
   Matrix<double,3,4> tfm;
   Quaterniond q0,q1;
   q0 = nd0.qrot;
   transformW2F(tfm,nd0.trans,q0);
   trans.head(3) = tfm*nd1.trans;
   trans(3) = 1.0;
   q1 = nd1.qrot;
   qr = q0.inverse()*q1;
   qr.normalize();
   if (qr.w() < 0)
     qr.coeffs() = -qr.coeffs();
 }

Eigen::Quaternion<T> naive_slerp(const Eigen::Quaternion<T>& input, const Eigen::Quaternion<T>& target, T amt){
  Eigen::Quaternion<T> result;
	
  if (amt==T(0)) {
    return input;
  } else if (amt==T(1)) {
    return target;
  }
	
  int bflip = 0;
  T dot_prod = input.dot(target);
  T a, b;
	
  //clamp
  dot_prod = (dot_prod < -1) ? -1 : ((dot_prod > 1) ? 1 : dot_prod);
	
  // if B is on opposite hemisphere from A, use -B instead
  if (dot_prod < 0.0) {
    dot_prod = -dot_prod;
    bflip = 1;
  }
	
  T cos_angle = acos(dot_prod);
  if(ABS(cos_angle) > QUAT_EPSILON) {
    T sine = sin(cos_angle);
    T inv_sine = 1./sine;
		
    a = sin(cos_angle*(1.-amt)) * inv_sine;
    b = sin(cos_angle*amt) * inv_sine;
		
    if (bflip) { b = -b; }
  } else {
    // nearly the same;
    // approximate without trigonometry
    a = amt;
    b = 1.-amt;
  }
	
  result.w = a*input.w + b*target.w;
  result.x = a*input.x + b*target.x;
  result.y = a*input.y + b*target.y;
  result.z = a*input.z + b*target.z;
	
  result.normalize();
  return result;
}

IGL_INLINE void igl::two_axis_valuator_fixed_up(
  const int w,
  const int h,
  const double speed,
  const Eigen::Quaternion<Scalardown_quat> & down_quat,
  const int down_x,
  const int down_y,
  const int mouse_x,
  const int mouse_y,
  Eigen::Quaternion<Scalarquat> & quat)
{
  using namespace Eigen;
  Matrix<Scalarquat,3,1> axis(0,1,0);
  quat = down_quat *
    Quaternion<Scalarquat>(
      AngleAxis<Scalarquat>(
        PI*((Scalarquat)(mouse_x-down_x))/(Scalarquat)w*speed/2.0,
        axis.normalized()));
  quat.normalize();
  {
    Matrix<Scalarquat,3,1> axis(1,0,0);
    if(axis.norm() != 0)
    {
      quat =
        Quaternion<Scalarquat>(
          AngleAxis<Scalarquat>(
            PI*(mouse_y-down_y)/(Scalarquat)h*speed/2.0,
            axis.normalized())) * quat;
      quat.normalize();
    }
  }
}

    /**
     * @brief Computes the trackball's rotation, using stored initial and final position vectors.
     */
    void computeRotationAngle (void)
    {
        //Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.

        if(initialPosition.norm() > 0) {
            initialPosition.normalize();
        }
        if(finalPosition.norm() > 0) {
            finalPosition.normalize();
        }

        //cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;

        Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);

        if(rotationAxis.norm() != 0) {
            rotationAxis.normalize();
        }

        float dot = initialPosition.dot(finalPosition);

        float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.

        Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));

        quaternion = q * quaternion;
        quaternion.normalize();
    }

//.........这里部分代码省略.........
#endif

    J0.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y());  // qpmean * ds0'/dy
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z());  // qpmean * ds0'/dz
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,5) = qrn.vec();

    // transpose
    J0t = J0.transpose();

    //  cout << endl << "J0 " << ndr << endl << J0 << endl;

    // Jacobians wrt second frame parameters
    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p1: zero
    J1.block<3,3>(0,3).setZero();

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J1.block<3,3>(3,0).setZero();


    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = q0'*s1*q1

    Eigen::Quaternion<double> qrc;
    qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qrc = qpmean*qrc*qr1;       // mean' * qr0' * qr1
    qrc.normalize();

    //    cout << endl << "QRC  : " << qrc.coeffs().transpose() << endl;

    double dq = 1.0e-8;
    double wdq = 1.0 - dq*dq;
    qr1.coeffs() = Vector4d(dq,0,0,wdq);
    //    cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;    
    //    cout << "QRdx:  " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;

    // dqdx
    qrn.coeffs() = Vector4d(1,0,0,0);
    qrn = qrc*qrn;

    //    cout << "J1dx:  " << qrn.coeffs().transpose() << endl;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(0,1,0,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(0,0,1,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,5) = qrn.vec();

    // transpose
    J1t = J1.transpose();

    //  cout << endl << "J1 " << nd1 << endl << J1 << endl;

  };

  void get3DMoments(vector<Point> feature_points, int feat_index, int index)
  {
    //    for(int i=0; i<feature_points.size(); i++)
    //   ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
    //ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
    
    //Extract the indices for the points in the point cloud data
    pcl::PointIndices point_indices;
     
    for(int i=0; i<feature_points.size(); i++)
      {
	//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
	point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
      }
    
    //ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
    Eigen::Vector4f centroid;
    Eigen::Matrix3f covariance_matrix;
    
    // Estimate the XYZ centroid
    pcl::compute3DCentroid (pcl_in, point_indices, centroid); 
#ifdef DEBUG
    ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif

    //ROS_INFO("Computing Covariance ");
    //Compute the centroid and the covariance of the points
    computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
    
    //Print the 3D Moments
    //ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
    std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif

    for(int i=0; i<3; i++)
      {
	feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
	for(int j=0; j<3; j++)
	  {
	    feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
	  }
      }

    //Get the principal components and the quaternion
    Eigen::Matrix3f evecs;
    Eigen::Vector3f evals;
    pcl::eigen33 (covariance_matrix, evecs, evals);
    
    Eigen::Matrix3f rotation;
    rotation.row (0) = evecs.col (0);
    rotation.row (1) = evecs.col (1);
    //rotation.row (2) = evecs.col (2);
    rotation.row (2) = rotation.row (0).cross (rotation.row (1));
    //rotation.transposeInPlace ();
#ifdef DEBUG
    std::cerr << "Rotation matrix: " << endl;
    std::cerr << rotation << endl;
    std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif

    rotation.transposeInPlace ();
    Eigen::Quaternion<float> qt (rotation);
    qt.normalize ();

    //Publish Marker
    visualization_msgs::Marker marker;	
    
    marker.header.frame_id = "/openni_rgb_optical_frame";
    marker.header.stamp = ros::Time().now();
    marker.ns = "Triangulation";
    marker.id = index+1;	
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(5);		
    
    //centroid position
    marker.type = visualization_msgs::Marker::SPHERE;
    
    
    marker.pose.position.x = centroid(0);
    marker.pose.position.y = centroid(1);
    marker.pose.position.z = centroid(2);	
    marker.pose.orientation.x = qt.x();
    marker.pose.orientation.y = qt.y();
    marker.pose.orientation.z = qt.z();
    marker.pose.orientation.w = qt.w();			
    
    marker.scale.x = sqrt(evals(0)) *2;
    marker.scale.y =  sqrt(evals(1)) *2;
    marker.scale.z =  sqrt(evals(2)) *2;
    
    //red
    marker.color.a = 0.5;
    marker.color.r = rand()/((double)RAND_MAX + 1);
    marker.color.g = rand()/((double)RAND_MAX + 1);
    marker.color.b = rand()/((double)RAND_MAX + 1);
    object_pose_marker_pub_.publish(marker);	
    
  }