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

void VideoIMUFusionDevice::handleIMUVelocity(
    const OSVR_TimeValue &timestamp, const OSVR_AngularVelocityReport &report) {

    using namespace osvr::util::eigen_interop;
    Eigen::Quaterniond q = map(report.state.incrementalRotation);
    Eigen::Vector3d rot;
    if (q.w() >= 1. || q.vec().isZero(1e-10)) {
        rot = Eigen::Vector3d::Zero();
    } else {
#if 0
        auto magnitude = q.vec().blueNorm();
        rot = (q.vec() / magnitude * (2. * std::atan2(magnitude, q.w()))) /
              report.state.dt;
#else
        auto angle = std::acos(q.w());
        rot = q.vec().normalized() * angle * 2 / report.state.dt;
#endif
    }

    m_fusion.handleIMUVelocity(timestamp, rot);
    if (m_fusion.running()) {
        sendMainPoseReport();
    }
    sendVelocityReport();
}

/*!
* \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;
}

Eigen::Quaterniond pick_positive(const Eigen::Quaterniond& other) {
  if (other.w() < 0) {
    return Eigen::Quaterniond(-other.w(), -other.x(), -other.y(), -other.z());
  } else {
    return other;
  }
}

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();
}

Eigen::Matrix3d FeatureModel::dR_by_dqz(const Eigen::Quaterniond &q)
{
  Eigen::Matrix3d M;

  M << -2*q.z(), -2*q.w(), 2*q.x(),
        2*q.w(), -2*q.z(), 2*q.y(),
        2*q.x(), 2*q.y(), 2*q.z();

  return  M;
}

void printQuatAngles(Eigen::Quaterniond& quat)
{
	double heading = atan2(2*quat.y()*quat.w()-2*quat.x()*quat.z(), 1 -2 * quat.y()*quat.y()-2*quat.z()*quat.z());

	double attitude = asin(2*quat.x()*quat.y() + 2 *quat.z()*quat.w());

	double bank = atan2(2*quat.x()*quat.w()- 2 * quat.y()*quat.z(), 1 - 2 *quat.x() * quat.x() -2 * quat.z() * quat.z());

	cout << "heading: " << Angle(heading).deg() << " attitude: " << Angle(attitude).deg() << " bank: " << Angle(bank).deg() << endl;
}

static inline Eigen::Vector3d
_quat_to_roll_pitch_yaw(const Eigen::Quaterniond&q)
{
  double roll_a = 2 * (q.w() * q.x() + q.y() * q.z());
  double roll_b = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
  double roll = atan2(roll_a, roll_b);

  double pitch_sin = 2 * (q.w() * q.y() - q.z() * q.x());
  double pitch = asin(pitch_sin);

  double yaw_a = 2 * (q.w() * q.z() + q.x() * q.y());
  double yaw_b = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
  double yaw = atan2(yaw_a, yaw_b);
  return Eigen::Vector3d(roll, pitch, yaw);
}

void RigidBody::set_attitude(Eigen::Quaterniond _attitude)
{
	attitude.w() = _attitude.w(); 
	attitude.x() = _attitude.x(); 
	attitude.y() = _attitude.y(); 
	attitude.z() = _attitude.z(); 
}

static void subject_publish_callback(const ViconDriver::Subject &subject)
{
  static std::set<std::string> defined_msgs;

  std::string msgname = "vicon_" + subject.name;
  if(defined_msgs.find(msgname) == defined_msgs.end())
  {
    if(pthread_mutex_trylock(&ipc_mutex) == 0)
    {
      IPC_defineMsg(msgname.c_str(), IPC_VARIABLE_LENGTH, ViconSubject::getIPCFormat());
      pthread_mutex_unlock(&ipc_mutex);
      defined_msgs.insert(msgname);
    }
    else
      return;
  }

  if(loadCalib(subject.name))
  {
    Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
    current_pose.translate(Eigen::Vector3d(subject.translation));
    current_pose.rotate(Eigen::Quaterniond(subject.rotation));
    current_pose = current_pose*calib_pose[subject.name];
    const Eigen::Vector3d position(current_pose.translation());
    const Eigen::Quaterniond rotation(current_pose.rotation());

    ViconSubject subject_ipc;
    subject_ipc.time_sec = subject.time_usec/1000000; // Integer division
    subject_ipc.time_usec = subject.time_usec - subject_ipc.time_sec*1000000;
    subject_ipc.frame_number = subject.frame_number;
    subject_ipc.name = const_cast<char*>(subject.name.c_str());
    subject_ipc.occluded = subject.occluded;
    subject_ipc.position[0] = position.x();
    subject_ipc.position[1] = position.y();
    subject_ipc.position[2] = position.z();
    subject_ipc.orientation[0] = rotation.x();
    subject_ipc.orientation[1] = rotation.y();
    subject_ipc.orientation[2] = rotation.z();
    subject_ipc.orientation[3] = rotation.w();
    subject_ipc.num_markers = subject.markers.size();
    subject_ipc.markers = new ViconMarker[subject_ipc.num_markers];
    for(int i = 0; i < subject_ipc.num_markers; i++)
    {
      subject_ipc.markers[i].name =
          const_cast<char*>(subject.markers[i].name.c_str());
      subject_ipc.markers[i].subject_name =
          const_cast<char*>(subject.markers[i].subject_name.c_str());
      subject_ipc.markers[i].position[0] = subject.markers[i].translation[0];
      subject_ipc.markers[i].position[1] = subject.markers[i].translation[1];
      subject_ipc.markers[i].position[2] = subject.markers[i].translation[2];
      subject_ipc.markers[i].occluded = subject.markers[i].occluded;
    }
    if(pthread_mutex_trylock(&ipc_mutex) == 0)
    {
      IPC_publishData(msgname.c_str(), &subject_ipc);
      pthread_mutex_unlock(&ipc_mutex);
    }
    delete[] subject_ipc.markers;
  }
}

geometry_msgs::PoseStamped MutualPoseEstimation::generatePoseMessage(const Eigen::Vector3d &position, Eigen::Matrix3d rotation){

    geometry_msgs::PoseStamped estimated_position;
    estimated_position.header.frame_id = "ardrone_base_frontcam";//ardrone_base_link

    //estimated_position.pose.position.x = position[2] + 0.21; // z
    //estimated_position.pose.position.y = -position[0];  // x
    //estimated_position.pose.position.z = -position[1]; //-y
    estimated_position.pose.position.x = position[0] + 0.21;
    estimated_position.pose.position.y = position[1];
    estimated_position.pose.position.z = position[2];

    /*rotation = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
               * rotation;*/

    Eigen::Quaterniond q = Eigen::Quaterniond(rotation);
//    estimated_position.pose.orientation.x = q.z();
//    estimated_position.pose.orientation.y = -q.x();
//    estimated_position.pose.orientation.z = -q.y();
//    estimated_position.pose.orientation.w = q.w();
    estimated_position.pose.orientation.x = q.x();
    estimated_position.pose.orientation.y = q.y();
    estimated_position.pose.orientation.z = q.z();
    estimated_position.pose.orientation.w = q.w();
    return estimated_position;
}

Eigen::Quaterniond VizHelperNode::rotateSatelliteFrame(const iri_common_drivers_msgs::SatellitePseudorange &sat, ros::Time time_ros)
{
    Eigen::Quaterniond rotation;

    Eigen::Vector3d satVelocity(sat.v_x * scale, sat.v_y * scale, sat.v_z * scale);

    //quaternione che fa ruotare l'asse x in modo che combaci con il vettore velocita'
    rotation.setFromTwoVectors(Eigen::Vector3d::UnitX(), satVelocity);


    geometry_msgs::Quaternion odom_quat;
    odom_quat.x = rotation.x();
    odom_quat.y = rotation.y();
    odom_quat.z = rotation.z();
    odom_quat.w = rotation.w();


    // Publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = time_ros;
    odom_trans.header.frame_id = WORLD_FRAME;				//frame padre
    odom_trans.child_frame_id = getSatelliteFrame(sat.sat_id);	//frame figlio (che sto creando ora)
    odom_trans.transform.translation.x = sat.x * scale;//traslazione dell'origine
    odom_trans.transform.translation.y = sat.y * scale;
    odom_trans.transform.translation.z = sat.z * scale;
    odom_trans.transform.rotation = odom_quat;							//rotazione

    //send the transform
    transBroadcaster.sendTransform(odom_trans);


    return rotation;
}

void VizHelperNode::publishOdometry(const iri_common_drivers_msgs::SatellitePseudorange &sat, const Eigen::Quaterniond &rotation, ros::Time time_ros)
{
    /// publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = time_ros;
    odom.header.frame_id = WORLD_FRAME;

    //set the position
    odom.pose.pose.position.x = sat.x * scale;
    odom.pose.pose.position.y = sat.y * scale;
    odom.pose.pose.position.z = sat.z * scale;

    //set the orientation
    odom.pose.pose.orientation.x = rotation.x();
    odom.pose.pose.orientation.y = rotation.y();
    odom.pose.pose.orientation.z = rotation.z();
    odom.pose.pose.orientation.w = rotation.w();

    //set the velocity
    odom.child_frame_id = getSatelliteFrame(sat.sat_id);
    odom.twist.twist.linear.x = sat.v_x * scale;
    odom.twist.twist.linear.y = sat.v_y * scale;
    odom.twist.twist.linear.z = sat.v_z * scale;
    odom.twist.twist.angular.x = 0;
    odom.twist.twist.angular.y = 0;
    odom.twist.twist.angular.z = 0;

    //publish the message
    //TODO fare i singoli odom publisher
    // odomPub[index].publish(odom);
    odomAllPub.publish(odom);
}

 void quaternionEigenToTF(const Eigen::Quaterniond& e, tf::Quaternion& t)
 {
   t[0] = e.x();
   t[1] = e.y();
   t[2] = e.z();
   t[3] = e.w();
 }

/**
 * @function pose2Affine3d
 */
Eigen::Affine3d DartLoader::pose2Affine3d( urdf::Pose _pose ) {
    Eigen::Quaterniond quat;
    _pose.rotation.getQuaternion(quat.x(), quat.y(), quat.z(), quat.w());
    Eigen::Affine3d transform(quat);
    transform.translation() = Eigen::Vector3d(_pose.position.x, _pose.position.y, _pose.position.z);
    return transform;
}

void App::getRobotState(drc::robot_state_t& robot_state_msg, int64_t utime_in, Eigen::VectorXd q, std::vector<std::string> jointNames){
  robot_state_msg.utime = utime_in;

  // Pelvis Pose:
  robot_state_msg.pose.translation.x =q(0);
  robot_state_msg.pose.translation.y =q(1);
  robot_state_msg.pose.translation.z =q(2);

  Eigen::Quaterniond quat = euler_to_quat( q(3), q(4), q(5));

  robot_state_msg.pose.rotation.w = quat.w();
  robot_state_msg.pose.rotation.x = quat.x();
  robot_state_msg.pose.rotation.y = quat.y();
  robot_state_msg.pose.rotation.z = quat.z();

  robot_state_msg.twist.linear_velocity.x  = 0;
  robot_state_msg.twist.linear_velocity.y  = 0;
  robot_state_msg.twist.linear_velocity.z  = 0;
  robot_state_msg.twist.angular_velocity.x = 0;
  robot_state_msg.twist.angular_velocity.y = 0;
  robot_state_msg.twist.angular_velocity.z = 0;

  // Joint States:
  for (size_t i = 0; i < jointNames.size(); i++)  {
    robot_state_msg.joint_name.push_back( jointNames[i] );
    robot_state_msg.joint_position.push_back( q(i) );
    robot_state_msg.joint_velocity.push_back( 0);
    robot_state_msg.joint_effort.push_back( 0 );
  }
  robot_state_msg.num_joints = robot_state_msg.joint_position.size();
}