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

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

bool checkRange(const Eigen::Quaternion<DerivedA>& qq,
  double minVal, double maxVal)
{
    assert(minVal < maxVal);

    if (std::isnan(qq.w()) || std::isnan(qq.x()) || std::isnan(qq.y()) || std::isnan(qq.z()))
    {
        std::stringstream ss;
        ss << "Quaternion [" << qq.w() << ", " << qq.x() << ", " << qq.y() << ", " << qq.z()  << "] contains NaN values!\n";
        ROS_WARN_STREAM(ss.str());
        return false;
    }

    if (qq.w() > maxVal || qq.w() < minVal ||
        qq.x() > maxVal || qq.x() < minVal ||
        qq.y() > maxVal || qq.y() < minVal ||
        qq.z() > maxVal || qq.z() < minVal)
    {
        std::stringstream ss;
        ss << "Quaternion contains terms out of range:\n"
          << " - w: [" << qq.w() << ", " << qq.x() << ", " << qq.y() << ", " << qq.z()  << "]\n"
          << " - minVal: " << minVal << "\n"
          << " - maxVel: " << maxVal;
        ROS_WARN_STREAM(ss.str());
    }

    return true;
}

 void computeEr(const Eigen::Quaternion<double>& q, Eigen::MatrixXd& Er)
 {
   Er.resize(4, 3);
   Er << -q.x(), -q.y(), -q.z(),
          q.w(),  q.z(), -q.y(),
         -q.z(),  q.w(),  q.x(),
          q.y(), -q.x(),  q.w();
   Er = 0.5 * Er;
 }

Eigen::Matrix3f
motion_controller::qToRotation(Eigen::Quaternion<float> q)
{
    Eigen::Matrix3f temp;

    temp << 1-2*(q.y()*q.y()+q.z()*q.z()), 2*(q.x()*q.y()-q.w()*q.z())  , 2*(q.w()*q.y()+q.x()*q.z())  ,
            2*(q.x()*q.y()+q.w()*q.z())  , 1-2*(q.x()*q.x()+q.z()*q.z()), 2*(q.y()*q.z()-q.w()*q.x())  ,
            2*(q.x()*q.z()-q.w()*q.y())  , 2*(q.y()*q.z()+q.w()*q.x())  , 1-2*(q.x()*q.x()+q.y()*q.y());
    return temp;
}

    SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose
        Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i
        Matrix<3> iniOrientationEKF;
        iniOrientationEKF = tag::quaternionToMatrix(attivec);

        Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in
                              0, -1, 0, // a world frame which pointing downward.
                              0, 0, -1);

        SE3<> camWorld = SE3<>();
        Matrix<3> rotation;
        if (tracker_->attitude_get)
            rotation = iniOrientationEKF; //
        else
            rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i
        camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric)

        Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc
        Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation();
        camWorld.get_translation()[0] = 0.0;//twc[0]; //twc
        camWorld.get_translation()[1] = 0.0;//twc[1];
        camWorld.get_translation()[2] = twc[2];

        camWorld = camWorld.inverse();//Tcw

        cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl;
//        cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl;
        return camWorld;
    }

void HLManager::start()
{
	ros::Rate rate(10);

	while (nh.ok())
	{
		if (fixValidated)
		{
			tf::Transform transform;
			transform.setOrigin(tf::Vector3(originLon, originLat, 0));
			transform.setRotation(tf::createQuaternionFromRPY(0,0,0));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/worldLatLon", "/world"));
			transform.setOrigin(tf::Vector3(0, 0, 0));
			Eigen::Quaternion<float> q;
			labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q);
			transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "local"));
		}
		this->safetyTest();
		this->step();
		hlMessagePub.publish(hlDiagnostics);
		rate.sleep();
		ros::spinOnce();
	}
}

void HLManager::onVTTwist(const geometry_msgs::TwistStamped::ConstPtr& twist)
{
	if (mode == circle || mode == vtManual)
	{
		//\todo Generalize this 0.1 with Ts.
		s +=twist->twist.linear.x*0.1;

		//Circle
		if (s>=2*circleRadius*M_PI) s=s-2*circleRadius*M_PI;
		else if (s<0) s=2*circleRadius*M_PI-s;

		double xRabbit = point.point.x + circleRadius*cos(s/circleRadius);
		double yRabbit = point.point.y + circleRadius*sin(s/circleRadius);
		double gammaRabbit=labust::math::wrapRad(s/circleRadius)+M_PI/2;

		tf::Transform transform;
		transform.setOrigin(tf::Vector3(xRabbit, yRabbit, 0));
		Eigen::Quaternion<float> q;
		labust::tools::quaternionFromEulerZYX(0,0,gammaRabbit, q);
		transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
		broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "serret_frenet_frame"));

		auv_msgs::NavStsPtr msg(new auv_msgs::NavSts());
		msg->position.north = xRabbit;
		msg->position.east = yRabbit;
		msg->body_velocity.x = twist->twist.linear.x;
		msg->orientation.yaw = gammaRabbit;
		sfPub.publish(msg);
	}
}

bool CloudMatcher::match(modular_cloud_matcher::MatchClouds::Request& req, modular_cloud_matcher::MatchClouds::Response& res)
{
	// get and check reference
	size_t referenceGoodCount;
	DP referenceCloud(rosMsgToPointMatcherCloud(req.reference, referenceGoodCount));
	const unsigned referencePointCount(req.reference.width * req.reference.height);
	const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
	
	if (referenceGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reference cloud");
		return false;
	}
	if (referenceGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")");
	}
	
	// get and check reading
	size_t readingGoodCount;
	DP readingCloud(rosMsgToPointMatcherCloud(req.readings, readingGoodCount));
	const unsigned readingPointCount(req.readings.width * req.readings.height);
	const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
	
	if (readingGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reading cloud");
		return false;
	}
	if (readingGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
	}
	
	// call icp
	TP transform;
	try 
	{
		transform = icp(readingCloud, referenceCloud);
		ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
	}
	catch (PM::ConvergenceError error)
	{
		ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
		return false;
	}
	
	// fill return value
	res.transform.translation.x = transform.coeff(0,3);
	res.transform.translation.y = transform.coeff(1,3);
	res.transform.translation.z = transform.coeff(2,3);
	const Eigen::Quaternion<Scalar> quat(Matrix3(transform.block(0,0,3,3)));
	res.transform.rotation.x = quat.x();
	res.transform.rotation.y = quat.y();
	res.transform.rotation.z = quat.z();
	res.transform.rotation.w = quat.w();
	
	return true;
}

void quaternion2vector(const Eigen::Quaternion<T_qt> &quat_In, std::vector<T_vec> &quat_Vec )
{
    quat_Vec.resize(4,T_vec(0.0));
    quat_Vec[0] = T_vec( quat_In.w() );
    quat_Vec[1] = T_vec( quat_In.x() );
    quat_Vec[2] = T_vec( quat_In.y() );
    quat_Vec[3] = T_vec( quat_In.z() );
};

void YouBot::updateWheels(const ros::TimerEvent& e)
{
  try
  {
    if(q_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(tau_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(!gazebo_interface_wheel_->subscribed()) return;

    Eigen::VectorXd q = gazebo_interface_wheel_->getJointStates();
    robot_->updateWheel(q);

    Eigen::Vector3d base_pos;
    base_pos << q_base_[0], q_base_[1], 0.0;
    Eigen::Quaternion<double> base_ori;
    base_ori.x() = 0.0;
    base_ori.y() = 0.0;
    base_ori.z() = sin(0.5 * q_base_[2]);
    base_ori.w() = cos(0.5 * q_base_[2]);

    robot_->updateBase(base_pos, base_ori);

    Eigen::VectorXd v_base;
    controller_->computeBaseVelocityFromTorque(tau_base_, v_base, 3);

    Eigen::VectorXd v_wheel;
    controller_->computeWheelVelocityFromBaseVelocity(v_base, v_wheel);

    Eigen::VectorXd q_wheel_d = robot_->getMobility()->update_rate * v_wheel;

    std::vector<Eigen::Quaternion<double> > quat_d;
    for(unsigned int i = 0; i < q_wheel_d.rows(); ++i)
    {
      double rad = q_wheel_d[i];

      Eigen::Quaternion<double> quat;
      quat.x() = 0.0;
      quat.y() = sin(0.5 * rad);
      quat.z() = 0.0;
      quat.w() = cos(0.5 * rad);

      quat_d.push_back(quat);
    }

    gazebo_interface_wheel_->rotateLink(quat_d);
  }
  catch(ahl_robot::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_ctrl::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}

    void rpyToQuaternion(const Eigen::Vector3d& rpy, Eigen::Quaternion<double>& q)
    {
      double a = rpy.coeff(0);
      double b = rpy.coeff(1);
      double g = rpy.coeff(2);

      double sin_b_half = sin(0.5 * b);
      double cos_b_half = cos(0.5 * b);
      double diff_a_g_half = 0.5 * (a - g);
      double sum_a_g_half = 0.5 * (a + g);

      q.x() = sin_b_half * cos(diff_a_g_half);
      q.y() = sin_b_half * sin(diff_a_g_half);
      q.z() = cos_b_half * sin(sum_a_g_half);
      q.w() = cos_b_half * cos(sum_a_g_half);
    }

base::Vector6d AuvMotion::gravity_buoyancy(const Eigen::Quaternion<double> q)
{
    base::Vector6d gravitybuoyancy;
  
    double mass;
    control->nodes->getNodeMass(vehicle_id ,&mass);
    double buoyancy = _buoyancy_force;
    base::Vector3d pos = control->nodes->getPosition(vehicle_id);
  
    // In Quaternion form
    float e1 = q.x();
    float e2 = q.y();
    float e3 = q.z();
    float e4 = q.w();
    float xg = 0.0;
    float yg = 0.0;
    float zg = 0.0;
    float xb = centerOfBuoyancy[0];
    float yb = centerOfBuoyancy[1];
    float zb = centerOfBuoyancy[2];;

    gravitybuoyancy(0) = 0.0;
    gravitybuoyancy(1) = 0.0;
    
    if(pos[2] + centerOfBuoyancy[2] < 0.0){ //The vehicle is completly under water
      gravitybuoyancy(2) = buoyancy - mass;
    }else if(pos[2] < 0.0 && centerOfBuoyancy[2] != 0){ //The vehicle is partwise underwater -> apply buoyancy partwise
      gravitybuoyancy(2) = (buoyancy * (-pos[2]/ centerOfBuoyancy[2] ) ) - mass;
    }else{ //The vehicle is over the surface -> no buoyancy
      gravitybuoyancy(2) = -mass;
    }
    
    //Angular buoancy forces
    gravitybuoyancy(3) = ((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((yg*mass)-(yb*buoyancy)))+(2*((e4*e1)+(e2*e3))*((zg*mass)-(zb*buoyancy)));
    gravitybuoyancy(4) =-((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((xg*mass)-(xb*buoyancy)))+(2*((e4*e2)-(e1*e3))*((zg*mass)-(zb*buoyancy)));
    gravitybuoyancy(5) =-(2*((e4*e1)+(e2*e3))*((xg*mass)-(xb*buoyancy)))-(2*((e4*e2)-(e1*e3))*((yg*mass)-(yb*buoyancy)));
    
    //Convert angular forces to world frame
    gravitybuoyancy.block<3,1>(3,0) = q * gravitybuoyancy.block<3,1>(3,0);  
    
    return gravitybuoyancy;
}

void publishNode(unsigned int index, Eigen::Matrix<double,4,1> trans, 
                Eigen::Quaternion<double> fq,
                const frame_common::CamParams &cp,
                bool isFixed, sba::CameraNode &msg)
{ 
    msg.index = index;
    
    msg.transform.translation.x = trans.x();
    msg.transform.translation.y = trans.y();
    msg.transform.translation.z = trans.z();
    msg.transform.rotation.x = fq.x();
    msg.transform.rotation.y = fq.y();
    msg.transform.rotation.z = fq.z();
    msg.transform.rotation.w = fq.w();
    msg.fx = cp.fx;
    msg.fy = cp.fy;
    msg.cx = cp.cx;
    msg.cy = cp.cy;
    msg.baseline = cp.tx;
    msg.fixed = isFixed;
}

    bool operator()(T const* const* parameters, T* residuals) const {
        // Map spline
        UniformSpline<T> spline(spline_dt_, spline_offset_);
        for (size_t i=0; i < num_knots_; ++i) {
            spline.add_knot((T*) &parameters[i][0]);
        }

        Eigen::Matrix<T, 3, 1> landmark(T(5.0), T(1.0), T(3.0));
        Eigen::Matrix<T, 3, 1> expected(T(0.0), T(0.0), T(1.0));


        // Distance to center
        Sophus::SE3Group<T> P;
        Eigen::Matrix<T, 4, 4> dP, d2P;
        for (size_t i=0; i < eval_times_.size(); ++i) {
            spline.evaluate(T(eval_times_[i]), P, dP, d2P);
            Eigen::Matrix<T, 3, 1> X_spline = P.inverse() * landmark;
            Eigen::Matrix<T, 3, 1> diff = X_spline - expected;
            residuals[3*i + 0] = diff(0);
            residuals[3*i + 1] = diff(1);
            residuals[3*i + 2] = diff(2);
        }

        const size_t poff = 3 * eval_times_.size();

        // Constant angular difference
        for (size_t i=1; i < eval_times_.size(); ++i) {
            SE3Group<T> P0, P1, delta;
            spline.evaluate(T(eval_times_[i-1]), P0, dP, d2P);
            spline.evaluate(T(eval_times_[i]), P1, dP, d2P);
            delta = P0.inverse() * P1;
            Eigen::Quaternion<T> q = delta.unit_quaternion();
            residuals[poff + 4*(i - 1) + 0] = T(5.0) * (q.w() - T(cos(0.5 * expected_angular_difference_)));
            residuals[poff + 4*(i - 1) + 1] = T(5.0) * (q.x() - T(sin(0.5 * expected_angular_difference_)));
            residuals[poff + 4*(i - 1) + 2] = T(5.0) * q.y(); //- 0;
            residuals[poff + 4*(i - 1) + 3] = T(5.0) * q.z(); //- 0;
        }

        return true;
    }

      bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
      {
        assert(q_urdf.size()==7);
        assert(q_sot.size()==6);
        // *********  RPY to Quat *********
        const double r = q_sot[3];
        const double p = q_sot[4];
        const double y = q_sot[5];
        const Eigen::AngleAxisd  rollAngle(r, Eigen::Vector3d::UnitX());
        const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
        const Eigen::AngleAxisd   yawAngle(y, Eigen::Vector3d::UnitZ());
        const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;

        q_urdf[0 ]=q_sot[0 ]; //BASE
        q_urdf[1 ]=q_sot[1 ];
        q_urdf[2 ]=q_sot[2 ];
        q_urdf[3 ]=quat.x();
        q_urdf[4 ]=quat.y();
        q_urdf[5 ]=quat.z();
        q_urdf[6 ]=quat.w();

        return true;
      }