int main(int argc, char** argv)
{
ros::init(argc, argv, "test2d");
ros::NodeHandle node;
tf::TransformListener t(ros::Duration(20));
tf::StampedTransform tr_o, tr_i;
double a_test(0);
double b_test(0);
double theta_test(0);
double nu_theta(1);
double nu_trans(1);
ROS_INFO_STREAM("waiting for initial transforms");
while (node.ok())
{
ros::Time now(ros::Time::now());
//ROS_INFO_STREAM(now);
if (t.waitForTransform(baseLinkFrame, now, baseLinkFrame, now, odomFrame, ros::Duration(0.1)))
break;
//ROS_INFO("wait");
//ros::Duration(0.1).sleep();
}
ROS_INFO_STREAM("got first odom to baseLink");
while (node.ok())
{
ros::Time now(ros::Time::now());
//ROS_INFO_STREAM(now);
if (t.waitForTransform(kinectFrame, now, kinectFrame, now, worldFrame, ros::Duration(0.1)))
break;
//ROS_INFO("wait");
//ros::Duration(0.1).sleep();
}
ROS_INFO_STREAM("got first world to kinect");
sleep(10);
ros::Rate rate(0.5);
while (node.ok())
{
// sleep
rate.sleep();
// get parameters from transforms
ros::Time curTime(ros::Time::now());
ros::Time lastTime = curTime - ros::Duration(10);
//ROS_INFO_STREAM("curTime: " << curTime << ", lastTime: " << lastTime);
t.waitForTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, ros::Duration(3));
t.waitForTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, ros::Duration(3));
t.lookupTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, tr_o);
//ROS_INFO_STREAM("odom to baselink: trans: " << tr_o.getOrigin() << ", rot: " << tr_o.getRotation());
const double alpha_o_tf = tr_o.getOrigin().x();
const double beta_o_tf = tr_o.getOrigin().y();
const double theta_o = 2*atan2(tr_o.getRotation().z(), tr_o.getRotation().w());
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;
// Step 7:
// P = (I - K*H)*P
// H is still I, so (I-K) * P
xdot.covariance[0] *= -K[0];
xdot.covariance[7] *= -K[7];
xdot.covariance[14] *= -K[14];
xdot.covariance[21] *= -K[21];
xdot.covariance[28] *= -K[28];
xdot.covariance[35] *= -K[35];
// Populate Message
odom_msg->pose.pose.position.x = tr.getOrigin( ).x( );
odom_msg->pose.pose.position.y = tr.getOrigin( ).y( );
odom_msg->pose.pose.position.z= tr.getOrigin( ).z( );
tf::quaternionTFToMsg( tr.getRotation( ), odom_msg->pose.pose.orientation );
odom_msg->twist = xdot;
if( local_frame )
{
// Rotate velocity vector to the local frame
tf::Vector3 tmp;
tf::Vector3 tmp2;
tf::vector3MsgToTF( odom_msg->twist.twist.linear, tmp );
tf::vector3MsgToTF( odom_msg->twist.twist.angular, tmp2 );
tf::vector3TFToMsg( tf::quatRotate( curr_quat.inverse( ), tmp ), odom_msg->twist.twist.linear );
tf::vector3TFToMsg( tf::quatRotate( curr_quat.inverse( ), tmp2 ), odom_msg->twist.twist.angular );
}
// Done, Publish
odom_pub.publish( odom_msg );
// Record when this message was for next time
last_delta_twist.twist.linear.x = odom_msg->twist.twist.linear.x - last_twist.twist.linear.x;
last_delta_twist.twist.linear.y = odom_msg->twist.twist.linear.y - last_twist.twist.linear.y;
last_delta_twist.twist.linear.z = odom_msg->twist.twist.linear.z - last_twist.twist.linear.z;
last_delta_twist.twist.angular.x = odom_msg->twist.twist.angular.x - last_twist.twist.angular.x;
last_delta_twist.twist.angular.y = odom_msg->twist.twist.angular.y - last_twist.twist.angular.y;
last_delta_twist.twist.angular.z = odom_msg->twist.twist.angular.z - last_twist.twist.angular.z;
last_transform = tr;
last_twist = odom_msg->twist;
last_quat = curr_quat;
};