C++ Quaterniond类代码示例

void pubOdometry()
{
    nav_msgs::Odometry odom;
    {
        odom.header.stamp = _last_imu_stamp;
        odom.header.frame_id = "map";

        odom.pose.pose.position.x = x(0);
        odom.pose.pose.position.y = x(1);
        odom.pose.pose.position.z = x(2);

        Quaterniond q;
        q = RPYtoR(x(3), x(4), x(5)).block<3,3>(0, 0);
        odom.pose.pose.orientation.x = q.x();
        odom.pose.pose.orientation.y = q.y();
        odom.pose.pose.orientation.z = q.z();
        odom.pose.pose.orientation.w = q.w();
    }

    //ROS_WARN("[update] publication done");
    ROS_WARN_STREAM("[final] b_g = " << x.segment(_B_G_BEG, _B_G_LEN).transpose());
    ROS_WARN_STREAM("[final] b_a = " << x.segment(_B_A_BEG, _B_A_LEN).transpose() << endl);
    ///ROS_WARN_STREAM("[final] cov_x = " << endl << cov_x << endl);
    odom_pub.publish(odom);
}

// SLERP interpolation between two quaternions
Quaterniond Quaterniond::slerp( const Quaterniond &b, 
                              H3DDouble frac ) const {
  Quaterniond a = *this;
  H3DDouble alpha = a.dotProduct(b);
  
  if ( alpha < 0 ) {
    a = -a;
    alpha = -alpha;
  }
  
  H3DDouble scale;
  H3DDouble invscale;
  
  if ( ( 1 - alpha ) >= Constants::f_epsilon) {  
    // spherical interpolation
    H3DDouble theta = acos( alpha );
    H3DDouble sintheta = 1 / sin( theta );
    scale = sin( theta * (1-frac) ) * sintheta;
    invscale = sin( theta * frac ) * sintheta;
  }
  else { 
    // linear interploation
    scale = 1 - frac;
    invscale = frac;
  }
  
  return ( a * scale) + ( b * invscale);
}    

  bool VertexCam::read(std::istream& is)
  {
    // first the position and orientation (vector3 and quaternion)
    Vector3d t;
    for (int i=0; i<3; i++){
      is >> t[i];
    }
    Vector4d rc;
    for (int i=0; i<4; i++) {
      is >> rc[i];
    }
    Quaterniond r;
    r.coeffs() = rc;
    r.normalize();


    // form the camera object
    SBACam cam(r,t);

    // now fx, fy, cx, cy, baseline
    double fx, fy, cx, cy, tx;

    // try to read one value
    is >>  fx;
    if (is.good()) {
      is >>  fy >> cx >> cy >> tx;
      cam.setKcam(fx,fy,cx,cy,tx);
    } else{

void doCapsuleSphereTest(double capsuleHeight, double capsuleRadius,
						 const Vector3d& capsulePosition, const Quaterniond& capsuleQuat,
						 double sphereRadius, const Vector3d& spherePosition, const Quaterniond& sphereQuat,
						 bool hasContacts, double depth,
						 const Vector3d& sphereProjection = Vector3d::Zero(),
						 const Vector3d& expectedNorm = Vector3d::Zero())
{
	std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(
		makeCapsuleRepresentation(capsuleHeight, capsuleRadius, capsuleQuat, capsulePosition),
		makeSphereRepresentation(sphereRadius, sphereQuat, spherePosition));

	CapsuleSphereDcdContact calc;
	calc.calculateContact(pair);
	EXPECT_EQ(hasContacts, pair->hasContacts());

	if (pair->hasContacts())
	{
		std::shared_ptr<Contact> contact(pair->getContacts().front());

		EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
		EXPECT_NEAR(depth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
		EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
		EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());

		Vector3d capsuleLocalNormal = capsuleQuat.inverse() * expectedNorm;
		Vector3d penetrationPoint0(sphereProjection - capsuleLocalNormal * capsuleRadius);
		Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
		Vector3d penetrationPoint1(sphereLocalNormal * sphereRadius);
		EXPECT_TRUE(eigenEqual(penetrationPoint0, contact->penetrationPoints.first.rigidLocalPosition.getValue()));
		EXPECT_TRUE(eigenEqual(penetrationPoint1, contact->penetrationPoints.second.rigidLocalPosition.getValue()));
	}
}

void KeyFrame::updateLoopConnection(Vector3d relative_t, Quaterniond relative_q, double relative_yaw)
{
    Eigen::Matrix<double, 8, 1> connected_info;
    connected_info <<relative_t.x(), relative_t.y(), relative_t.z(),
    relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
    relative_yaw;
    loop_info = connected_info;
}

void KeyFrame::addConnection(int index, KeyFrame* connected_kf, Vector3d relative_t, Quaterniond relative_q, double relative_yaw)
{
    Eigen::Matrix<double, 8, 1> connected_info;
    connected_info <<relative_t.x(), relative_t.y(), relative_t.z(),
    relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
    relative_yaw;
    connection_list.push_back(make_pair(index, connected_info));
}

Matrix3d quaternion2mat(Quaterniond q)
{
  Matrix3d m;
  double a = q.w(), b = q.x(), c = q.y(), d = q.z();
  m << a*a + b*b - c*c - d*d, 2*(b*c - a*d), 2*(b*d+a*c),
       2*(b*c+a*d), a*a - b*b + c*c - d*d, 2*(c*d - a*b),
       2*(b*d - a*c), 2*(c*d+a*b), a*a-b*b - c*c + d*d;
  return m;
}

Matrix4d virtuose::getPose(float f[7]) {
    Matrix4d m;
    Vec3d pos(f[1], f[2], f[0]);
    Quaterniond q;
    q.setValue(f[4], f[5], f[3]);
    m.setRotate(q);
    m.setTranslate(pos);
    return m;
}

void BenchmarkNode::runFromFolder(int start)
{
  ofstream outfile;
  outfile.open ("/home/worxli/Datasets/data/associate_unscaled.txt");
  // outfile.open ("/home/worxli/data/test/associate_unscaled.txt");
  for(int img_id = start;;++img_id)
  {

    // load image
    std::stringstream ss;
    ss << "/home/worxli/Datasets/data/img/color" << img_id << ".png";
	  // ss << "/home/worxli/data/test/img/color" << img_id << ".png";
    std::cout << "reading image " << ss.str() << std::endl;
    cv::Mat img(cv::imread(ss.str().c_str(), 0));

    // end loop if no images left
    if(img.empty())
      break;

    assert(!img.empty());

    // process frame
    vo_->addImage(img, img_id);

    // display tracking quality
    if(vo_->lastFrame() != NULL)
    {
      int id = vo_->lastFrame()->id_;
    	std::cout << "Frame-Id: " << id << " \t"
                  << "#Features: " << vo_->lastNumObservations() << " \t"
                  << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";

    	// access the pose of the camera via vo_->lastFrame()->T_f_w_.
	//std::cout << vo_->lastFrame()->T_f_w_ << endl;

	//std::count << vo_->lastFrame()->pos() << endl;
  Quaterniond quat = vo_->lastFrame()->T_f_w_.unit_quaternion();
  Vector3d trans = vo_->lastFrame()->T_f_w_.translation();

	outfile << trans.x() << " "
          << trans.y() << " "
          << trans.z() << " " 

          << quat.x()  << " " 
          << quat.y()  << " " 
          << quat.z()  << " " 
          << quat.w()  << " " 

          << "depth/mapped" << img_id << ".png "
          << "img/color" << img_id << ".png\n";

    }
  }
  outfile.close();
}

void Visualization::transformPose(geometry_msgs::Pose& pose,
    const Vector3d& trans, const Quaterniond& rot)
{
  Vector3d pos;
  pos.x() = pose.position.x;
  pos.y() = pose.position.y;
  pos.z() = pose.position.z;

  pos = rot * pos + trans;

  pose.position.x = pos.x();
  pose.position.y = pos.y();
  pose.position.z = pos.z();

  Quaterniond orientation;
  orientation.x() = pose.orientation.x;
  orientation.y() = pose.orientation.y;
  orientation.z() = pose.orientation.z;
  orientation.w() = pose.orientation.w;

  orientation = rot * orientation;

  pose.orientation.x = orientation.x();
  pose.orientation.y = orientation.y();
  pose.orientation.z = orientation.z();
  pose.orientation.w = orientation.w();
}

void mouse_drag(int mouse_x, int mouse_y)
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;
  bool tw_using = TwMouseMotion(mouse_x,mouse_y);

  if(is_rotating)
  {
    glutSetCursor(GLUT_CURSOR_CYCLE);
    Quaterniond q;
    auto & camera = s.camera;
    switch(rotation_type)
    {
      case ROTATION_TYPE_IGL_TRACKBALL:
      {
        // Rotate according to trackball
        igl::trackball<double>(
          width,
          height,
          2.0,
          down_camera.m_rotation_conj.coeffs().data(),
          down_x,
          down_y,
          mouse_x,
          mouse_y,
          q.coeffs().data());
          break;
      }
      case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
      {
        // Rotate according to two axis valuator with fixed up vector
        two_axis_valuator_fixed_up(
          width, height,
          2.0,
          down_camera.m_rotation_conj,
          down_x, down_y, mouse_x, mouse_y,
          q);
        break;
      }
      default:
        break;
    }
    switch(center_type)
    {
      default:
      case CENTER_TYPE_ORBIT:
        camera.orbit(q.conjugate());
        break;
      case CENTER_TYPE_FPS:
        camera.turn_eye(q.conjugate());
        break;
    }
  }
}

void
ArTrackOrientation::setKeyFrame(unsigned frame, const Quaterniond &orientation)
{
VectorN<double,4> base;
base[0]=orientation.q0();
base[1]=orientation.q1();
base[2]=orientation.q2();
base[3]=orientation.q3();

((ArTrackOrientationInterpolator *)_track)->setKeyFrame(frame,base);
}

inline Pose::Pose(const Quaterniond& orientation,
	          const Vector3d& position)
	: m_orientation(orientation)
	, m_position(position)
{
	CHECK(std::fabs(orientation.norm() - 1) < 1e-15)
		<< "Your quaternion is not normalized:"
		<< orientation.norm();
	// Or, one may prefer
	m_orientation.normalize();
}

/*! Get the position of the location relative to its body in 
 *  the J2000 ecliptic coordinate system.
 */
Vector3d Location::getPlanetocentricPosition(double t) const
{
    if (parent == NULL)
    {
        return position.cast<double>();
    }
    else
    {
        Quaterniond q = parent->getEclipticToBodyFixed(t);
        return q.conjugate() * position.cast<double>();
    }
}

geometry_msgs::Quaternion eigenQuaterniondToTfQuaternion( Quaterniond q ){

    geometry_msgs::Quaternion tfq;

    tfq.x = q.x();
    tfq.y = q.y();
    tfq.z = q.z();
    tfq.w = q.w();

    return tfq;

}