C++ eigen::Quaterniond类代码示例

void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) {
  const double q0 = q.w();
  const double q1 = q.x();
  const double q2 = q.y();
  const double q3 = q.z();
  roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
  pitch = asin(2*(q0*q2-q3*q1));
  yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}

void UpperBodyPlanner::msgPose2Eigen(const geometry_msgs::Pose& input, Eigen::Vector3d& translation, Eigen::Quaterniond& q) {
    translation(0) = input.position.x;
    translation(1) = input.position.y;
    translation(2) = input.position.z;
    q.w() = input.orientation.w;
    q.x() = input.orientation.x;
    q.y() = input.orientation.y;
    q.z() = input.orientation.z;
}

Eigen::Vector3d QuaternionToExponentialMap(const Eigen::Quaterniond& quaternion)
{
	Eigen::Vector3d vec = quaternion.vec();
    if (vec.norm() < ITOMP_EPS)
		return Eigen::Vector3d::Zero();

	double theta = 2.0 * std::acos(quaternion.w());
	vec.normalize();
	return theta * vec;
}

Eigen::Isometry3d KDLToEigen(KDL::Frame tf)
{
  Eigen::Isometry3d tf_out;
  tf_out.setIdentity();
  tf_out.translation()  << tf.p[0], tf.p[1], tf.p[2];
  Eigen::Quaterniond q;
  tf.M.GetQuaternion(q.x(), q.y(), q.z(), q.w());
  tf_out.rotate(q);
  return tf_out;
}

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
	mPoseMap.lock();
    vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 
    					     OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;


    Eigen::Quaterniond globalQ;
    globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
    Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
    vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
                              globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
    globalPoseMap[t] = globalPose;
    lastP = globalP;
    lastQ = globalQ;

    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time(t);
    pose_stamped.header.frame_id = "world";
    pose_stamped.pose.position.x = lastP.x();
    pose_stamped.pose.position.y = lastP.y();
    pose_stamped.pose.position.z = lastP.z();
    pose_stamped.pose.orientation.x = lastQ.x();
    pose_stamped.pose.orientation.y = lastQ.y();
    pose_stamped.pose.orientation.z = lastQ.z();
    pose_stamped.pose.orientation.w = lastQ.w();
    global_path.header = pose_stamped.header;
    global_path.poses.push_back(pose_stamped);

    mPoseMap.unlock();
}

/*!
* \brief affine3d2UrdfPose converts an  Eigen affine 4x4 matrix  o represent the pose into a urdf pose 
* vparam pose   eigen Affine3d pose 
* \return   urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const  Eigen::Affine3d &pose) {
    RCS::Pose p;
    p.getOrigin().setX(pose.translation().x());
    p.getOrigin().setY(pose.translation().y());
    p.getOrigin().setZ(pose.translation().z());

    Eigen::Quaterniond q (pose.rotation());
    tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
    //std::cout <<  "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;

    p.setRotation(qtf);
    //std::cout <<  "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;

#if 0
    MatrixEXd m = pose.rotation();
    Eigen::Quaterniond q = EMatrix2Quaterion(m);

    Eigen::Quaterniond q(pose.rotation());
    p.getRotation().setX(q.x());
    p.getRotation().setY(q.y());
    p.getRotation().setZ(q.z());
    p.getRotation().setW(q.w());
    #endif
   return p;
}

void updateVizMarker(int id, Eigen::Vector3d _pose, Eigen::Quaterniond _attitude)
{
	marker[id].pose.position.x = _pose(0);
	marker[id].pose.position.y = _pose(1);
	marker[id].pose.position.z = _pose(2);
	marker[id].pose.orientation.x = _attitude.x();
	marker[id].pose.orientation.y = _attitude.y();
	marker[id].pose.orientation.z = _attitude.z();
	marker[id].pose.orientation.w = _attitude.w();
	pub_body[id].publish( marker[id] );
}

/**
 * Recursion method to be used with traverseTreeTopDown() and recursion parameters
 * of type *Vector3RecursionParams*.
 *
 * Re-arranges the joint-transform of the recursion link's *parent joint*, along with
 * the link's visual/collision/intertial rotations, such that all joints rotate around the axis
 * given in the recursion parameters vector.
 */
int allRotationsToAxisCB(urdf_traverser::RecursionParamsPtr& p)
{
    urdf_traverser::LinkPtr link = p->getLink();
    if (!link)
    {
        ROS_ERROR("allRotationsToAxis: NULL link passed");
        return -1;
    }

    Vector3RecursionParams::Ptr param = baselib_binding_ns::dynamic_pointer_cast<Vector3RecursionParams>(p);
    if (!param)
    {
        ROS_ERROR("Wrong recursion parameter type");
        return -1;
    }

    urdf_traverser::JointPtr joint = link->parent_joint;
    if (!joint)
    {
        ROS_INFO_STREAM("allRotationsToAxis: Joint for link " << link->name << " is NULL, so this must be the root joint");
        return 1;
    }

    Eigen::Vector3d axis = param->vec;

    Eigen::Quaterniond alignAxis;
    if (urdf_traverser::jointTransformForAxis(joint, axis, alignAxis))
    {
        Eigen::Vector3d rotAxis(joint->axis.x, joint->axis.y, joint->axis.z);
        // ROS_INFO_STREAM("Transforming axis "<<rotAxis<<" for joint "<<joint->name<<" with transform "<<urdf_traverser::EigenTransform(alignAxis));

        urdf_traverser::applyTransform(joint, urdf_traverser::EigenTransform(alignAxis), false);

        // the link has to receive the inverse transform, so it stays at the original position
        Eigen::Quaterniond alignAxisInv = alignAxis.inverse();
        urdf_traverser::applyTransform(link, urdf_traverser::EigenTransform(alignAxisInv), true);

        // we also have to fix the child joint's (1st order child joints) transform
        // to correct for this transformation.
        for (std::vector<urdf_traverser::JointPtr>::iterator pj = link->child_joints.begin();
                pj != link->child_joints.end(); pj++)
        {
            urdf_traverser::applyTransform(*pj, urdf_traverser::EigenTransform(alignAxisInv), true);
        }

        // finally, set the rotation axis to the target
        joint->axis.x = axis.x();
        joint->axis.y = axis.y();
        joint->axis.z = axis.z();
    }

    // all good, indicate that recursion can continue
    return 1;
}

double quaternion_get_yaw(const Eigen::Quaterniond &q)
{
	// to match equation from:
	// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
	const double &q0 = q.w();
	const double &q1 = q.x();
	const double &q2 = q.y();
	const double &q3 = q.z();

	return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
}

double igl::angular_distance(
  const Eigen::Quaterniond & A,
  const Eigen::Quaterniond & B)
{
  using namespace igl;
  assert(fabs(A.norm()-1)<FLOAT_EPS && "A should be unit norm");
  assert(fabs(B.norm()-1)<FLOAT_EPS && "B should be unit norm");
  //// acos is always in [0,2*pi)
  //return acos(fabs(A.dot(B)));
  return fmod(2.*acos(A.dot(B)),2.*PI);
}

Eigen::Quaterniond UpperBodyPlanner::Rmat2Quaternion(const Eigen::Matrix3d& inMat_) {
    Eigen::Quaterniond outQ;
    KDL::Rotation convert;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            convert.data[3 * i + j] = inMat_(i,j);
        }
    }
    convert.GetQuaternion(outQ.x(), outQ.y(), outQ.z(), outQ.w());
    return outQ;
}

void planning_models::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
  if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
  {
    Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
    Eigen::Quaterniond q;
    quatFromMsg(transform.transform.rotation, q);
    setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
  } else {
    ROS_ERROR("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
  }
}

void create_data(Eigen::MatrixXd &pa, Eigen::MatrixXd &pb) {
	int n_data = 7;
   	Eigen::MatrixXd pa0(3, n_data);	
   	Eigen::MatrixXd pb0(3, n_data);	
	
	pb0 << -0.7045189014502934,0.31652495664145264,-0.8913587885243552,0.4196143278053829,0.33125081405575785,-1.148712511573519,-0.7211957446166447,-0.4204243223315903,-0.8922857301575797,0.41556308950696674,-0.36760757371251074,-1.1630155401570457,-0.12535642300333297,0.26708755761917147,1.5095344824450356,0.9968448409012386,0.27593113974268946,1.2189108175890786,-0.28095118914331707,-0.40276257201497045,1.3669272703783852; 
	
	Eigen::Quaterniond q = Eigen::Quaterniond(2.86073, 0.0378363, 3.59752, 0.4211619).normalized();
	std::cout << "groundtruth-quaternion. w: " << q.w() << " x " << q.x() << " y: " << q.y() << " z " << q.z() << std::endl;
	pa = q.toRotationMatrix()*pb0;
	pb = pb0;
}

void robot_state::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
  if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
  {
    Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
    Eigen::Quaterniond q;
    tf::quaternionMsgToEigen(transform.transform.rotation, q);
    setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
  } else {
    logError("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
  }
}

void LCM2ROS::neckPitchHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const drc::neck_pitch_t* msg)
{
  ROS_ERROR("LCM2ROS got desired neck pitch");
  ihmc_msgs::HeadOrientationPacketMessage mout;
  Eigen::Quaterniond quat = euler_to_quat(0, msg->pitch, 0);
  mout.trajectory_time = 1;
  mout.orientation.w = quat.w();
  mout.orientation.x = quat.x();
  mout.orientation.y = quat.y();
  mout.orientation.z = quat.z();
  mout.unique_id = msg->utime;
  neck_orientation_pub_.publish(mout);
}