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

//.........这里部分代码省略.........
		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 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;
//.........这里部分代码省略.........