C++ tf::Quaternion类代码示例

tf::Matrix3x3 JPCT_Math::quatToMatrix(tf::Quaternion q) {
    tf::Matrix3x3 matrix; 
    float norm = magnitudeSquared(q);
    float s = (double)norm > 0.0?2.0f / norm:0.0F;
    float xs = q.x() * s;
    float ys = q.y() * s;
    float zs = q.z() * s;
    float xx = q.x() * xs;
    float xy = q.x() * ys;
    float xz = q.x() * zs;
    float xw = q.w() * xs;
    float yy = q.y() * ys;
    float yz = q.y() * zs;
    float yw = q.w() * ys;
    float zz = q.z() * zs;
    float zw = q.w() * zs;
    matrix[0][0] = 1.0F - (yy + zz);
    matrix[1][0] = xy - zw;
    matrix[2][0] = xz + yw;
    matrix[0][1] = xy + zw;
    matrix[1][1] = 1.0F - (xx + zz);
    matrix[2][1] = yz - xw;
    matrix[0][2] = xz - yw;
    matrix[1][2] = yz + xw;
    matrix[2][2] = 1.0F - (xx + yy);

    return matrix;
}

void Hand_filter::update(tf::Vector3 p, tf::Quaternion& q){

    if(b_first){
        p_filter_buffer.push_back(p);
        q_filter_buffer.push_back(q);
        if(p_filter_buffer.size() == p_filter_buffer.capacity()){
            b_first = false;
            ROS_INFO("====== hand filter ======");
           // ROS_INFO("buffer full: %d",p_filter_buffer.size());
            ROS_INFO("p: %f %f %f",p.x(),p.y(),p.z());
            ROS_INFO("q: %f %f %f %f",q.x(),q.y(),q.z(),q.w());

            k_position(0) = p.x();k_position(1) = p.y(); k_position(2) = p.z();
            kalman_filter.Init(k_position);

            q_tmp = q;
            p_tmp = p;

        }
    }else{


        /// Orientation filter
       if(jumped(q,q_tmp,q_threashold)){
            ROS_INFO("q jumped !");
            q = q_tmp;
        }

       q_attractor(q,up);
       q = q_tmp.slerp(q,0.1);


       /// Position filter
        if(!jumped(p,p_tmp,p_threashold)){

            k_measurement(0) = p.x();
            k_measurement(1) = p.y();
            k_measurement(2) = p.z();

        }else{
            ROS_INFO("p jumped !");
            k_measurement(0) = p_tmp.x();
            k_measurement(1) = p_tmp.y();
            k_measurement(2) = p_tmp.z();
        }

        kalman_filter.Update(k_measurement,0.001);
        kalman_filter.GetPosition(k_position);
        p.setValue(k_position(0),k_position(1),k_position(2));



        q_tmp = q;
        p_tmp = p;

    }


}

/**
 * Convert tf::Quaternion to string
 */
template<> std::string toString<tf::Quaternion>(const tf::Quaternion& p_quat)
{
  std::stringstream ss;

  ss << "(" << p_quat.x() << ", " << p_quat.y() << ", " << p_quat.z() << ", " << p_quat.w() << ")";

  return ss.str();
}

/**
  * Packs current state in a odom message. Needs a quaternion for conversion.
  */
void pack_pose(tf::Quaternion& q, nav_msgs::Odometry& odom)
{
    q.setRPY(0, 0, _theta);

    odom.header.stamp = ros::Time::now();

    odom.pose.pose.position.x = _x;
    odom.pose.pose.position.y = _y;

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

void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){

    if(bFirst){

        origin_buffer.push_back(origin);
        orientation_buffer.push_back(orientation);

        if(origin_buffer.size() == origin_buffer.capacity()){
            bFirst = false;
            ROS_INFO("====== jump filter full ======");
        }

    }else{

        origin_tmp      = origin_buffer[origin_buffer.size()-1];
        orientation_tmp = orientation_buffer[orientation_buffer.size()-1];

        if(bDebug){
            std::cout<< "=== jum debug === " << std::endl;
            std::cout<< "p    : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
            std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
            std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;

            std::cout<< "q    : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() <<  "\t" << orientation.w() << std::endl;
            std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
            std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
        }

        /// Position jump
        if(jumped(origin,origin_tmp,origin_threashold)){
            ROS_INFO("position jumped !");
            origin = origin_tmp;
           // exit(0);
        }else{
            origin_buffer.push_back(origin);
        }

        /// Orientation jump
        if(jumped(orientation,orientation_tmp,orientation_threashold)){
            ROS_INFO("orientation jumped !");
            orientation = orientation_tmp;
            //exit(0);
        }else{
            orientation_buffer.push_back(orientation);
        }
    }
}

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    double r, p ,y;
    
    lastQuat = tf::Quaternion(msg->pose.pose.orientation.x,
                              msg->pose.pose.orientation.y,
                              msg->pose.pose.orientation.z,
                              msg->pose.pose.orientation.w);
    
    btMatrix3x3(lastQuat).getRPY(r, p, y);
    
    lastQuat.setRPY(r, p, y + M_PI / 2.0);
    
    lastOdom = *msg;
}

//.........这里部分代码省略.........
		const double alpha_o = cos(theta_o)*alpha_o_tf + sin(theta_o)*beta_o_tf;
		const double beta_o = -sin(theta_o)*alpha_o_tf + cos(theta_o)*beta_o_tf;
		t.lookupTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, tr_i);
		//ROS_INFO_STREAM("world to kinect: trans: " << tr_i.getOrigin() << ", rot: " << tr_i.getRotation());
		const double alpha_i_tf = tr_i.getOrigin().z();
		const double beta_i_tf = -tr_i.getOrigin().x();
		const double theta_i = 2*atan2(-tr_i.getRotation().y(), tr_i.getRotation().w());
		const double alpha_i = cos(theta_i)*alpha_i_tf + sin(theta_i)*beta_i_tf;
		const double beta_i = -sin(theta_i)*alpha_i_tf + cos(theta_i)*beta_i_tf;
		lastTime = curTime;
		
		ROS_WARN_STREAM("Input odom: ("<<alpha_o<<", "<<beta_o<<", "<<theta_o<<"), icp: ("\
				<<alpha_i<<", "<<beta_i<<", "<<theta_i<<")");
		if (abs(theta_i-theta_o) > max_diff_angle)
		{
			ROS_WARN_STREAM("Angle difference too big: " << abs(theta_i-theta_o));
			continue;
		}

		// compute correspondances, check values to prevent numerical instabilities
		const double R_denom = 2 * sin(theta_o);
		/*if (abs(R_denom) < limit_low)
		{
			ROS_WARN_STREAM("magnitude of R_denom too low: " << R_denom);
			continue;
		}*/
		const double kr_1 = (alpha_o * cos(theta_o) + alpha_o + beta_o * sin(theta_o));
		const double kr_2 = (beta_o * cos(theta_o) + beta_o - alpha_o * sin(theta_o));
		const double phi = atan2(kr_2, kr_1);
		const double r_1 = kr_1/R_denom;
		const double r_2 = kr_2/R_denom;
		const double R = sqrt(r_1*r_1 + r_2*r_2);
		const double kC_1 = (beta_i + beta_i * cos(theta_o) + alpha_i * sin(theta_o));
		const double kC_2 = (alpha_i + alpha_i * cos(theta_o) + beta_i * sin(theta_o));
		//const double xi = atan2(kC_2 + R_denom*b_test, kC_1 + R_denom*a_test);
		const double C_1 = kC_1 / R_denom;
		const double C_2 = kC_2 / R_denom;
		double xi(0);
		if (R_denom)
			xi = atan2(C_1 + a_test, C_2 + b_test);
		else
			xi = atan2(kC_1, kC_2);
		
		// compute new values 
		//double tmp_theta = M_PI/2 - phi  - xi;
		double tmp_theta = xi - phi;
		double tmp_a = R * sin(tmp_theta+phi) - C_1;
		double tmp_b = R * cos(tmp_theta+phi) - C_2;
		//tmp_theta = (tmp_theta<-M_PI)?tmp_theta+2*M_PI:((tmp_theta>M_PI)?tmp_theta-2*M_PI:tmp_theta);
		
		//const double V = sqrt((C_1+a_test)*(C_1+a_test)+(C_2+b_test)*(C_2+b_test));
		/*const double err = sqrt((a_test-tmp_a)*(a_test-tmp_a)+(b_test-tmp_b)*(b_test-tmp_b));
		const double err_pred = sqrt(R*R + V*V -2*R*V*cos(tmp_theta+phi-xi));
		if (abs(err-err_pred)>0.00001)
		{
		ROS_WARN_STREAM("Error="<<err<<" Computed="<<err_pred);
		ROS_WARN_STREAM("chosen: ("<<tmp_a<<", "<<tmp_b<<"); rejected: ("
				<<R*sin(tmp_theta+phi+M_PI)-C_1<<", "<<R*cos(tmp_theta+phi+M_PI)-C_2<<")");
		
		ROS_WARN_STREAM("R: "<<R<<", V: "<<V<<", C_1: "<<C_1<<", C_2: "<<C_2\
				<<", phi: "<<phi<<", xi: "<<xi<<", theta: "<<tmp_theta);
		}*/
		if (R>min_R_rot)
		{
			theta_test = atan2((1-nu_theta)*sin(theta_test)+nu_theta*sin(tmp_theta),
					(1-nu_theta)*cos(theta_test)+nu_theta*cos(tmp_theta));
			nu_theta = max(min_nu, 1/(1+1/nu_theta));
		}
		if (R<max_R_trans)
		{
			a_test = (1-nu_trans)*a_test + nu_trans*tmp_a;
			b_test = (1-nu_trans)*b_test + nu_trans*tmp_b;
			nu_trans = max(min_nu, 1/(1+1/nu_trans));
		}
		
		// compute transform
		const tf::Quaternion quat_trans = tf::Quaternion(a_test, b_test, 0, 1);
		const tf::Quaternion quat_test = tf::Quaternion(0, 0, sin(theta_test/2), cos(theta_test / 2));
		const tf::Quaternion quat_axes = tf::Quaternion(-0.5, 0.5, -0.5, 0.5);
		const tf::Quaternion quat_rot = quat_test*quat_axes;
		
		tf::Quaternion quat_tmp = quat_rot.inverse()*quat_trans*quat_rot;

		const tf::Vector3 vect_trans = tf::Vector3(quat_tmp.x(), quat_tmp.y(), 0);


		tf::Transform transform;
		transform.setRotation(quat_rot);
		transform.setOrigin(vect_trans);

		ROS_INFO_STREAM("Estimated transform: trans: " <<  a_test << ", " <<
				b_test << ", rot: " << 2*atan2(quat_test.z(), quat_test.w()));
	
		static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
baseLinkFrame, myKinectFrame));
	}
	
	return 0;
}

void InteractiveMarkerArrow::MakeInteractiveMarker(std::string intMarkerName, tf::Quaternion qx_control, tf::Quaternion qy_control, tf::Quaternion qz_control)
{	
    visualization_msgs::InteractiveMarker int_marker;
    int_marker.header.frame_id = "world";
    int_marker.scale = 0.1;
    int_marker.name = intMarkerName;
    
    geometry_msgs::Pose pose;    
    tfSrv->get("world", intMarkerName + "_control", pose);    
    int_marker.pose = pose;

    InteractiveMarkerArrow::MakeControl(int_marker);
    
    int_marker.controls[0].interaction_mode = 7;

    visualization_msgs::InteractiveMarkerControl control;
    control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
    
    tf::Transform tr;
    tfSrv->get("world", "viceGripRotation", tr);
    
    qx_control = tr.getRotation() * qx_control;
    qy_control = tr.getRotation() * qy_control;
    qz_control = tr.getRotation() * qz_control;

    control.orientation.w = qx_control.getW();
    control.orientation.x = qx_control.getX();
    control.orientation.y = qx_control.getY();
    control.orientation.z = qx_control.getZ();
    control.name = "rotate_x";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_x";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);

    control.orientation.w = qz_control.getW();
    control.orientation.x = qz_control.getX();
    control.orientation.y = qz_control.getY();
    control.orientation.z = qz_control.getZ();
    control.name = "rotate_z";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_z";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);

    control.orientation.w = qy_control.getW();
    control.orientation.x = qy_control.getX();
    control.orientation.y = qy_control.getY();
    control.orientation.z = qy_control.getZ();
    control.name = "rotate_y";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_y";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);
	    
    intMarkerSrv->insert(int_marker);      
    intMarkerSrv->setCallback(int_marker.name, _processFeedBackTemp(boost::bind(&InteractiveMarkerArrow::ProcessFeedback, this, _1)));
    intMarkerSrv->applyChanges();
}

//.........这里部分代码省略.........
						// need to convert to pose message so use
						if (fc_found)
						{
							// converting the rotation from a cv matrix to quaternion, first need it as a matrix3x3
							R_fc_tf[0][0] = R_fc.at<double>(0,0);
							R_fc_tf[0][1] = R_fc.at<double>(0,1);
							R_fc_tf[0][2] = R_fc.at<double>(0,2);
							R_fc_tf[1][0] = R_fc.at<double>(1,0);
							R_fc_tf[1][1] = R_fc.at<double>(1,1);
							R_fc_tf[1][2] = R_fc.at<double>(1,2);
							R_fc_tf[2][0] = R_fc.at<double>(2,0);
							R_fc_tf[2][1] = R_fc.at<double>(2,1);
							R_fc_tf[2][2] = R_fc.at<double>(2,2);
							std::cout << "Final R:\n" << R_fc << std::endl;
							
							// converting the translation to a vector 3
							T_fc_tf.setX(T_fc.at<double>(0,0));
							T_fc_tf.setY(T_fc.at<double>(0,1));
							T_fc_tf.setZ(T_fc.at<double>(0,2));
							std::cout << "Final T :\n" << T_fc << std::endl;
							
							// getting the rotation as a quaternion
							R_fc_tf.getRotation(Q_fc_tf);
							
							std::cout << "current orientation:" << "\n\tx:\t" << Q_fc_tf.getX() 
																<< "\n\ty:\t" << Q_fc_tf.getY() 
																<< "\n\tz:\t" << Q_fc_tf.getZ() 
																<< "\n\tw:\t" << Q_fc_tf.getW() 
																<< std::endl;
				
							std::cout << "norm of quaternion:\t" << Q_fc_tf.length() << std::endl;
							
							// getting the negated version of the quaternion for the check
							Q_fc_tf_negated = tf::Quaternion(-Q_fc_tf.getX(),-Q_fc_tf.getY(),-Q_fc_tf.getZ(),-Q_fc_tf.getW());
							
							std::cout << "negated orientation:" << "\n\tx:\t" << Q_fc_tf_negated.getX() 
																<< "\n\ty:\t" << Q_fc_tf_negated.getY() 
																<< "\n\tz:\t" << Q_fc_tf_negated.getZ() 
																<< "\n\tw:\t" << Q_fc_tf_negated.getW() 
																<< std::endl;
																
							std::cout << "norm of negated quaternion:\t" << Q_fc_tf_negated.length() << std::endl;
							
							// showing the last orientation
							std::cout << "last orientation:" << "\n\tx:\t" << Q_fc_tf_last.getX() 
															 << "\n\ty:\t" << Q_fc_tf_last.getY() 
															 << "\n\tz:\t" << Q_fc_tf_last.getZ() 
															 << "\n\tw:\t" << Q_fc_tf_last.getW() 
															 << std::endl;
																
							std::cout << "norm of last quaternion:\t" << Q_fc_tf_last.length() << std::endl;
							
							// checking if the quaternion has flipped
							Q_norm_current_diff = std::sqrt(std::pow(Q_fc_tf.getX() - Q_fc_tf_last.getX(),2.0)
														  + std::pow(Q_fc_tf.getY() - Q_fc_tf_last.getY(),2.0) 
														  + std::pow(Q_fc_tf.getZ() - Q_fc_tf_last.getZ(),2.0) 
														  + std::pow(Q_fc_tf.getW() - Q_fc_tf_last.getW(),2.0));
							
							std::cout << "current difference:\t" << Q_norm_current_diff << std::endl;
							
							Q_norm_negated_diff = std::sqrt(std::pow(Q_fc_tf_negated.getX() - Q_fc_tf_last.getX(),2.0)
														  + std::pow(Q_fc_tf_negated.getY() - Q_fc_tf_last.getY(),2.0) 
														  + std::pow(Q_fc_tf_negated.getZ() - Q_fc_tf_last.getZ(),2.0) 
														  + std::pow(Q_fc_tf_negated.getW() - Q_fc_tf_last.getW(),2.0));
							
							std::cout << "negated difference:\t" << Q_norm_negated_diff << std::endl;

bool Hand_filter::jumped(const tf::Quaternion& q_current,const tf::Quaternion& q_previous,const float threashold) const {
    tf::Vector3 axis = q_current.getAxis();

   // std::cout<< "axis: " << axis.x() << " " << axis.y() << " " << axis.z() << std::endl;
    return false;
}

int main (int argc, char** argv) {
    ros::init(argc, argv, "tp_serial");
    ros::NodeHandle n;
    ROS_INFO("Serial server is starting");

    ros::Time current_time;

    tf::TransformBroadcaster transform_broadcaster;

    ros::Subscriber ucCommandMsg; // subscript to "cmd_vel" for receive command
    ros::Publisher odom_pub; // publish the odometry
    ros::Publisher path_pub;

    nav_msgs::Odometry move_base_odom;
    nav_msgs::Path pathMsg;

    static tf::Quaternion move_base_quat;

    //Initialize fixed values for odom and tf message
    move_base_odom.header.frame_id = "odom";
    move_base_odom.child_frame_id = "base_robot";

    //Reset all covariance values
    move_base_odom.pose.covariance = boost::assign::list_of(WHEEL_COVARIANCE)(0)(0)(0)(0)(0)
                                     (0)(WHEEL_COVARIANCE)(0)(0)(0)(0)
                                     (0)(0)(999999)(0)(0)(0)
                                     (0)(0)(0)(999999)(0)(0)
                                     (0)(0)(0)(0)(999999)(0)
                                     (0)(0)(0)(0)(0)(WHEEL_COVARIANCE);
    move_base_odom.twist.covariance = boost::assign::list_of(WHEEL_COVARIANCE)(0)(0)(0)(0)(0)
                                      (0)(WHEEL_COVARIANCE)(0)(0)(0)(0)
                                      (0)(0)(999999)(0)(0)(0)
                                      (0)(0)(0)(999999)(0)(0)
                                      (0)(0)(0)(0)(999999)(0)
                                      (0)(0)(0)(0)(0)(WHEEL_COVARIANCE);
    pathMsg.header.frame_id = "odom";
    //----------ROS Odometry publishing------------------

    //----------Initialize serial connection
    ROS_INFO("Serial Initialing:\n Port: %s \n BaudRate: %d", DEFAULT_SERIALPORT, DEFAULT_BAUDRATE);
    int fd = -1;
    struct termios newtio;
    FILE *fpSerial = NULL;
    // read/write, not controlling terminal for process
    fd = open(DEFAULT_SERIALPORT, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd < 0) {
        ROS_ERROR("Serial Init: Could not open serial device %s", DEFAULT_SERIALPORT);
        return 0;
    }
    //set up new setting
    memset(&newtio, 0, sizeof(newtio));
    newtio.c_cflag = CS8 | CLOCAL | CREAD; //8bit, no parity, 1 stop bit
    newtio.c_iflag |= IGNBRK; //ignore break codition
    newtio.c_oflag = 0;	//all options off
    //newtio.c_lflag = ICANON; //process input as lines
    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 20; // byte readed per a time
    //active new setting
    tcflush(fd, TCIFLUSH);

    if (cfsetispeed(&newtio, BAUDMACRO) < 0 || cfsetospeed(&newtio, BAUDMACRO)) {
        ROS_ERROR("Serial Init: Failed to set serial BaudRate: %d", DEFAULT_BAUDRATE);
        close(fd);
        return 0;
    }
    ROS_INFO("Connection established with %s at %d.", DEFAULT_SERIALPORT, BAUDMACRO);
    tcsetattr(fd, TCSANOW, &newtio);
    tcflush(fd, TCIOFLUSH);

    // Open file as a standard I/O stream
    fpSerial = fdopen(fd, "r+");
    if (!fpSerial) {
        ROS_ERROR("Serial Init: Failed to open serial stream %s", DEFAULT_SERIALPORT);
        fpSerial = NULL;
    }

    //Creating message to talk with ROS
    //Subscribe to ROS message
    ucCommandMsg = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, ucCommandCallback);
    //Setup to publish ROS message
    odom_pub = n.advertise<nav_msgs::Odometry>("tp_robot/odom", 10);
    path_pub = n.advertise<nav_msgs::Path>("tp_robot/odomPath", 10);

    // An "adaptive" timer to maintain periodical communication with the MCU
    ros::Rate rate(FPS);
    uint8_t i = FPS;
    while (i--) {
        find_mean = true;
        rate.sleep();
        ros::spinOnce();
    }
    find_mean = false;

    //Loop for input
    while(ros::ok()) {
        //Process the callbacks
        ros::spinOnce();
        //Impose command and get back respone
        int res;
        cmd_frame[0] = 'f';
//.........这里部分代码省略.........

bool
ArucoMapping::processImage(cv::Mat input_image,cv::Mat output_image)
{
  aruco::MarkerDetector Detector;
  std::vector<aruco::Marker> temp_markers;

  //Set visibility flag to false for all markers
  for(size_t i = 0; i < num_of_markers_; i++)
      markers_[i].visible = false;

  // Save previous marker count
  marker_counter_previous_ = marker_counter_;

  // Detect markers
  Detector.detect(input_image,temp_markers,aruco_calib_params_,marker_size_);
    
  // If no marker found, print statement
  if(temp_markers.size() == 0)
    ROS_DEBUG("No marker found!");

  //------------------------------------------------------
  // FIRST MARKER DETECTED
  //------------------------------------------------------
  if((temp_markers.size() > 0) && (first_marker_detected_ == false))
  {
    //Set flag
    first_marker_detected_=true;

    // Detect lowest marker ID
    lowest_marker_id_ = temp_markers[0].id;
    for(size_t i = 0; i < temp_markers.size();i++)
    {
      if(temp_markers[i].id < lowest_marker_id_)
        lowest_marker_id_ = temp_markers[i].id;
    }


    ROS_DEBUG_STREAM("The lowest Id marker " << lowest_marker_id_ );

    // Identify lowest marker ID with world's origin
    markers_[0].marker_id = lowest_marker_id_;

    markers_[0].geometry_msg_to_world.position.x = 0;
    markers_[0].geometry_msg_to_world.position.y = 0;
    markers_[0].geometry_msg_to_world.position.z = 0;

    markers_[0].geometry_msg_to_world.orientation.x = 0;
    markers_[0].geometry_msg_to_world.orientation.y = 0;
    markers_[0].geometry_msg_to_world.orientation.z = 0;
    markers_[0].geometry_msg_to_world.orientation.w = 1;

    // Relative position and Global position
    markers_[0].geometry_msg_to_previous.position.x = 0;
    markers_[0].geometry_msg_to_previous.position.y = 0;
    markers_[0].geometry_msg_to_previous.position.z = 0;

    markers_[0].geometry_msg_to_previous.orientation.x = 0;
    markers_[0].geometry_msg_to_previous.orientation.y = 0;
    markers_[0].geometry_msg_to_previous.orientation.z = 0;
    markers_[0].geometry_msg_to_previous.orientation.w = 1;

    // Transformation Pose to TF
    tf::Vector3 position;
    position.setX(0);
    position.setY(0);
    position.setZ(0);

    tf::Quaternion rotation;
    rotation.setX(0);
    rotation.setY(0);
    rotation.setZ(0);
    rotation.setW(1);

    markers_[0].tf_to_previous.setOrigin(position);
    markers_[0].tf_to_previous.setRotation(rotation);

    // Relative position of first marker equals Global position
    markers_[0].tf_to_world=markers_[0].tf_to_previous;

    // Increase count
    marker_counter_++;

    // Set sign of visibility of first marker
    markers_[0].visible=true;

    ROS_INFO_STREAM("First marker with ID: " << markers_[0].marker_id << " detected");

    //First marker does not have any previous marker
    markers_[0].previous_marker_id = THIS_IS_FIRST_MARKER;
  }

  //------------------------------------------------------
  // FOR EVERY MARKER DO
  //------------------------------------------------------
  for(size_t i = 0; i < temp_markers.size();i++)
  {
    int index;
    int current_marker_id = temp_markers[i].id;

    //Draw marker convex, ID, cube and axis
//.........这里部分代码省略.........

	void MocapKalman::spinOnce( )
	{
		const static double dt = (double)r.expectedCycleTime( ).nsec / 1000000000 + r.expectedCycleTime( ).sec;

		static double K[36] = { 0 };
		static double F[36] = { 0 };
		static geometry_msgs::PoseWithCovariance residual;
		static geometry_msgs::Vector3 rpy;
		static tf::Quaternion curr_quat;
		static tf::StampedTransform tr;

		// Get the transform
		try
		{
			li.lookupTransform( frame_base, frame_id, ros::Time(0), tr );
		}
		catch( tf::TransformException ex )
		{
			ROS_INFO( "Missed a transform...chances are that we are still OK" );
			return;
		}
		if( tr.getOrigin( ).x( ) != tr.getOrigin( ).x( ) )
		{
			ROS_WARN( "NaN DETECTED" );
			return;
		}

		nav_msgs::OdometryPtr odom_msg( new nav_msgs::Odometry );

		odom_msg->header.frame_id = frame_base;
		odom_msg->header.stamp = ros::Time::now( );
		odom_msg->child_frame_id = frame_id;

		// Get the RPY from this iteration
		curr_quat = tr.getRotation( );
		tf::Matrix3x3( ( curr_quat * last_quat.inverse( ) ).normalize( ) ).getRPY(rpy.x, rpy.y, rpy.z);

		// Step 1:
		// x = F*x
		// We construct F based on the acceleration previously observed
		// We will assume that dt hasn't changed much since the last calculation
		F[0] = ( xdot.twist.linear.x < 0.001 ) ? 1 : ( xdot.twist.linear.x + last_delta_twist.twist.linear.x ) / xdot.twist.linear.x;
		F[7] = ( xdot.twist.linear.y < 0.001 ) ? 1 : ( xdot.twist.linear.y + last_delta_twist.twist.linear.y ) / xdot.twist.linear.y;
		F[14] = ( xdot.twist.linear.z < 0.001 ) ? 1 : ( xdot.twist.linear.z + last_delta_twist.twist.linear.z ) / xdot.twist.linear.z;
		F[21] = ( xdot.twist.angular.x < 0.001 ) ? 1 : ( xdot.twist.angular.x + last_delta_twist.twist.angular.x ) / xdot.twist.angular.x;
		F[28] = ( xdot.twist.angular.y < 0.001 ) ? 1 : ( xdot.twist.angular.y + last_delta_twist.twist.angular.y ) / xdot.twist.angular.y;
		F[35] = ( xdot.twist.angular.z < 0.001 ) ? 1 : ( xdot.twist.angular.z + last_delta_twist.twist.angular.z ) / xdot.twist.angular.z;

		// Step 2:
		// P = F*P*F' + Q
		// Since F is only populated on the diagonal, F=F'
		xdot.covariance[0] += linear_process_variance;
		xdot.covariance[7] += linear_process_variance;
		xdot.covariance[14] += linear_process_variance;
		xdot.covariance[21] += angular_process_variance;
		xdot.covariance[28] += angular_process_variance;
		xdot.covariance[35] += angular_process_variance;

		// Step 3:
		// y = z - H*x
		// Because x estimates z directly, H = I
		residual.pose.position.x = ( tr.getOrigin( ).x( ) - last_transform.getOrigin( ).x( ) ) / dt - xdot.twist.linear.x;
		residual.pose.position.y = ( tr.getOrigin( ).y( ) - last_transform.getOrigin( ).y( ) ) / dt - xdot.twist.linear.y;
		residual.pose.position.z = ( tr.getOrigin( ).z( ) - last_transform.getOrigin( ).z( ) ) / dt - xdot.twist.linear.z;
		// HACK for some odd discontinuity in the rotations...
		//if( rpy.x > .4 || rpy.y > .4 || rpy.z > .4 || rpy.x < -.4 || rpy.y < -.4 || rpy.z < -.4 )
		//	rpy.x = rpy.y = rpy.z = 0;
		residual.pose.orientation.x = rpy.x / dt - xdot.twist.angular.x;
		residual.pose.orientation.y = rpy.y / dt - xdot.twist.angular.y;
		residual.pose.orientation.z = rpy.z / dt - xdot.twist.angular.z;

		// Step 4:
		// S = H*P*H' + R
		// Again, since H = I, S is simply P + R
		residual.covariance[0] = xdot.covariance[0] + linear_observation_variance;
		residual.covariance[7] = xdot.covariance[7] + linear_observation_variance;
		residual.covariance[14] = xdot.covariance[14] + linear_observation_variance;
		residual.covariance[21] = xdot.covariance[21] + angular_observation_variance;
		residual.covariance[28] = xdot.covariance[28] + angular_observation_variance;
		residual.covariance[35] = xdot.covariance[35] + angular_observation_variance;

		// Step 5:
		// K = P*H'*S^(-1)
		// Again, since H = I, and since S is only populated along the diagonal,
		// we can invert each element along the diagonal
		K[0] = xdot.covariance[0] / residual.covariance[0];
		K[7] = xdot.covariance[7] / residual.covariance[7];
		K[14] = xdot.covariance[14] / residual.covariance[14];
		K[21] = xdot.covariance[21] / residual.covariance[21];
		K[28] = xdot.covariance[28] / residual.covariance[28];
		K[35] = xdot.covariance[35] / residual.covariance[35];

		// Step 6:
		// x = x + K*y
		xdot.twist.linear.x += K[0] * residual.pose.position.x;
		xdot.twist.linear.y += K[7] * residual.pose.position.y;
		xdot.twist.linear.z += K[14] * residual.pose.position.z;
		xdot.twist.angular.x += K[21] * residual.pose.orientation.x;
		xdot.twist.angular.y += K[28] * residual.pose.orientation.y;
		xdot.twist.angular.z += K[35] * residual.pose.orientation.z;
//.........这里部分代码省略.........

void ATTCallback (IvyClientPtr app, void* data , int argc, char **argv)
{
	double att_unit_coef= 0.0139882;
	double phi, theta, yaw;
		
	sscanf(argv[0],"%lf %lf %lf %*d %*d %*d",&phi,&theta,&yaw);
	phi*=att_unit_coef*DEG2RAD;
	theta*=-att_unit_coef*DEG2RAD;
	yaw*=-att_unit_coef*DEG2RAD;
	
	q.setRPY(phi,theta,yaw);
	
	//ROS_INFO("Phi %f; Theta %f; Yaw %f", phi,theta,yaw);
	//ROS_INFO("q1 %f; q2 %f; q3 %f; q4 %f", q.x(),q.y(),q.z(),q.w());

	imu_data.header.stamp = ros::Time::now();
	imu_data.orientation.x=q.x();
	imu_data.orientation.y=q.y();
	imu_data.orientation.z=q.z();
	imu_data.orientation.w=q.w();
	
	imu_message.publish(imu_data);
	
	//Only temporary until the rates are equal
	att_data.header.stamp = ros::Time::now();
	att_data.orientation.x=q.x();
	att_data.orientation.y=q.y();
	att_data.orientation.z=q.z();
	att_data.orientation.w=q.w();

	att_message.publish(att_data);
}

void MTMHaptics::convert_bodyForcetoSpatialForce(geometry_msgs::WrenchStamped &body_wrench){

    visualize_haptic_force(body_force_pub);
    rot_quat.setX(cur_mtm_pose.orientation.x);
    rot_quat.setY(cur_mtm_pose.orientation.y);
    rot_quat.setZ(cur_mtm_pose.orientation.z);
    rot_quat.setW(cur_mtm_pose.orientation.w);
    F7wrt0.setValue(body_wrench.wrench.force.x, body_wrench.wrench.force.y, body_wrench.wrench.force.z);
    rot_matrix.setRotation(rot_quat);
    F0wrt7 = rot_matrix.transpose() * F7wrt0;
    body_wrench.wrench.force.x = F0wrt7.x();
    body_wrench.wrench.force.y = F0wrt7.y();
    body_wrench.wrench.force.z = F0wrt7.z();
    visualize_haptic_force(spatial_force_pub);

}