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