MikeSands's profile - activity

If you haven't already, you might want to check out robot_pose_ekf which does pretty much what you're describing: combining odom and imu/gyro data to output a transform that is better than either sensor taken separately.

If you're set on making your own version of robot_pose_ekf--the first thing I would do is visualize the odom and imu rotation vectors separately (and simultaneously) in rviz to make sure your IMU is outputting correct rotations with respect to the odom.

Here's a simple node that inputs an Imu message and outputs a Pose message (for visual debugging)

Hello all,

Currently we've written a gyro-based odometry node for our robot. We have two encoders which provide the base of the odometry and then we have a gyro for correcting yaw (like when the wheels slip). The node broadcasts an odometry message as well as a stamped transform. The messages are correct as far as position and yaw are concerned. It is the transform that doesn't seem to be working correctly.

The transform tree is

/gyro_odom -> /base_link -> /UTM

When I look at our hokuyo scans in rviz I have the fixed frame and target frame set up so that the scans should stay in basically the same position while the robot moves around inside them. This works for translation but fails miserably for rotation. The scans get smeared rather than the tf tree just turning inside the scans.

Here is the code for broadcasting the transform:

// Now that all the calculations are done, we can pack all the data into
// odometry messages and tf.

// make the quaternion
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(_yaw);

// make the tf
geometry_msgs::TransformStamped odom_tf;
odom_tf.header.stamp = _current_time;
odom_tf.header.frame_id = _odom_frame_id;
odom_tf.child_frame_id = _base_link_frame_id;
odom_tf.transform.translation.x = _x;
odom_tf.transform.translation.y = _y;
odom_tf.transform.translation.z = 0.0;
odom_tf.transform.rotation = odom_quat;

// braodcast the tf
_tf_bc.sendTransform(odom_tf);

Does anyone have any ideas as to what might be causing this to fail?

Well I figured it out but I figured I'd post it up here in case anyone else runs into this issue. Even though inverse() is part of the MatrixXd type you still need to include Eigen/LU for it to complete the type and allow it to compile. (I only had Eigen/Core included)

Hi,

I'm working on writing a FastSLAM 2.0 implementation to use with our system. I have the line of code:

Eigen::Matrix3d sigma_x = (G_v[current_feature].transpose() * (Z[current_feature].inverse() * G_v[current_feature])) + motion_noise_.inverse();

G_v[current_feature] is a 2x3 matrix, Eigen::Matrix<double,2,3>
Z[current_feature] is a 2x2 matrix, Eigen::Matrix2d
motion_noise_ is a 3x3 matrix, Eigen::Matrix3d

So it becomes sigma_x = (3x2) * ((2x2) * (2x3)) + (3x3)

When I try to compile this I get the following error:

/home/msands/ros/smart_wheelchair/chair_SLAM/src/chair_SLAM.cpp:209: error: invalid use of incomplete type ‘const struct Eigen::internal::inverse_impl<eigen::matrix<double, 2,="" 2,="" 0,="" 2,="" 2=""> >’
  /home/msands/ros/geometry/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h:233: error: declaration of ‘const struct Eigen::internal::inverse_impl<eigen::matrix<double, 2,="" 2,="" 0,="" 2,="" 2=""> >’

It has something to do with the calling of inverse() but I can't figure out what it is. Anyone have any thoughts?

-Mike