Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix the coordinate transformation with odometry #58

Closed
wants to merge 1 commit into from
Closed

fix the coordinate transformation with odometry #58

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ lib/
docs/
msg_gen/
srv_gen/
csm/
*.*~
__init__.py
__init__.pyc
7 changes: 3 additions & 4 deletions laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ class LaserScanMatcher

tf::Transform base_to_laser_; // static, cached
tf::Transform laser_to_base_; // static, cached, calculated from base_to_laser_
tf::Transform last_used_odom_tf_, latest_odom_tf_, odom_increment_tf_;

ros::Publisher pose_publisher_;
ros::Publisher pose_stamped_publisher_;
Expand Down Expand Up @@ -138,14 +139,13 @@ class LaserScanMatcher
bool received_vel_;

tf::Transform f2b_; // fixed-to-base tf (pose of base frame in fixed frame)
tf::Transform f2b_kf_; // pose of the last keyframe scan in fixed frame
tf::Transform f2b_kf_; // pose of the base in fixed frame when last keyframe is taken

ros::Time last_icp_time_;

sensor_msgs::Imu latest_imu_msg_;
sensor_msgs::Imu last_used_imu_msg_;
nav_msgs::Odometry latest_odom_msg_;
nav_msgs::Odometry last_used_odom_msg_;

geometry_msgs::Twist latest_vel_msg_;

Expand Down Expand Up @@ -179,8 +179,7 @@ class LaserScanMatcher

bool newKeyframeNeeded(const tf::Transform& d);

void getPrediction(double& pr_ch_x, double& pr_ch_y,
double& pr_ch_a, double dt);
void getPrediction(tf::Transform& inc_tf, double dt);

void createTfFromXYTheta(double x, double y, double theta, tf::Transform& t);
};
Expand Down
95 changes: 43 additions & 52 deletions laser_scan_matcher/src/laser_scan_matcher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,9 @@ LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_privat

f2b_.setIdentity();
f2b_kf_.setIdentity();
odom_increment_tf_.setIdentity();
last_used_odom_tf_.setIdentity();
latest_odom_tf_.setIdentity();
input_.laser[0] = 0.0;
input_.laser[1] = 0.0;
input_.laser[2] = 0.0;
Expand Down Expand Up @@ -352,7 +355,6 @@ void LaserScanMatcher::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg
latest_odom_msg_ = *odom_msg;
if (!received_odom_)
{
last_used_odom_msg_ = *odom_msg;
received_odom_ = true;
}
}
Expand Down Expand Up @@ -431,7 +433,7 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the laser frame since the last scan
// of the laser in the laser frame since the last keyframe scan
// The computed correction is then propagated using the tf machinery

prev_ldp_scan_->odometry[0] = 0.0;
Expand All @@ -452,27 +454,23 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
// **** estimated change since last scan

double dt = (time - last_icp_time_).toSec();
double pr_ch_x, pr_ch_y, pr_ch_a;
getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);

// the predicted change of the laser's position, in the fixed frame

tf::Transform pr_ch;
createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);

// account for the change since the last kf, in the fixed frame

pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse());

// the predicted change of the laser's position, in the laser frame
// the predicted change of the base's position, in the base_frame
getPrediction(odom_increment_tf_, dt);

// we have already feed the keyframe's scan: prev_ldp_scan_,
// and the current scan: curr_ldp_scan into the sm_icp().
// now, we will need a prediction of how the current scan's pose
// will look like.

// the predicted change of the laser's position, in the key laser frame
tf::Transform pr_ch_l;
pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ;

pr_ch_l = laser_to_base_ * (f2b_kf_.inverse()) * f2b_ * odom_increment_tf_ * base_to_laser_;
input_.first_guess[0] = pr_ch_l.getOrigin().getX();
input_.first_guess[1] = pr_ch_l.getOrigin().getY();
input_.first_guess[2] = tf::getYaw(pr_ch_l.getRotation());


// If they are non-Null, free covariance gsl matrices to avoid leaking memory
if (output_.cov_x_m)
{
Expand All @@ -493,20 +491,19 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
// *** scan match - using point to line icp from CSM

sm_icp(&input_, &output_);
tf::Transform corr_ch;
tf::Transform corr_ch_l;

if (output_.valid)
{

// the correction of the laser's position, in the laser frame
tf::Transform corr_ch_l;
// the pose of the current laser scan, in the last key laser frame
createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
ROS_DEBUG("corr_ch_l:[%f, %f, %f]", corr_ch_l.getOrigin().x(),
corr_ch_l.getOrigin().y(),
tf::getYaw(corr_ch_l.getRotation()) );

// the correction of the base's position, in the base frame
corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;

// update the pose in the world frame
f2b_ = f2b_kf_ * corr_ch;
// update the base pose in the world frame
f2b_ = f2b_kf_ * base_to_laser_ * corr_ch_l * laser_to_base_;

// **** publish

Expand Down Expand Up @@ -606,18 +603,19 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
}
else
{
corr_ch.setIdentity();
corr_ch_l.setIdentity();
ROS_WARN("Error in scan matching");
}

// **** swap old and new

if (newKeyframeNeeded(corr_ch))
if (newKeyframeNeeded(corr_ch_l))
{
// generate a keyframe
ld_free(prev_ldp_scan_);
prev_ldp_scan_ = curr_ldp_scan;
f2b_kf_ = f2b_;
//EWINGROS_INFO("key!");
}
else
{
Expand Down Expand Up @@ -795,56 +793,49 @@ bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id)

// returns the predicted change in pose (in fixed frame)
// since the last time we did icp
void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y,
double& pr_ch_a, double dt)
void LaserScanMatcher::getPrediction(tf::Transform& inc_tf, double dt)
{
boost::mutex::scoped_lock(mutex_);

// **** base case - no input available, use zero-motion model
pr_ch_x = 0.0;
pr_ch_y = 0.0;
pr_ch_a = 0.0;

double pr_ch_x = 0.0;
double pr_ch_y = 0.0;
double pr_ch_a = 0.0;
// **** use velocity (for example from ab-filter)
if (use_vel_)
{
pr_ch_x = dt * latest_vel_msg_.linear.x;
pr_ch_y = dt * latest_vel_msg_.linear.y;
pr_ch_a = dt * latest_vel_msg_.angular.z;

if (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI;
else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI;
createTfFromXYTheta( pr_ch_x, pr_ch_y, pr_ch_a,
inc_tf );
}

// **** use wheel odometry
if (use_odom_ && received_odom_)
{
pr_ch_x = latest_odom_msg_.pose.pose.position.x -
last_used_odom_msg_.pose.pose.position.x;

pr_ch_y = latest_odom_msg_.pose.pose.position.y -
last_used_odom_msg_.pose.pose.position.y;

pr_ch_a = tf::getYaw(latest_odom_msg_.pose.pose.orientation) -
tf::getYaw(last_used_odom_msg_.pose.pose.orientation);

if (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI;
else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI;

last_used_odom_msg_ = latest_odom_msg_;
createTfFromXYTheta(latest_odom_msg_.pose.pose.position.x,
latest_odom_msg_.pose.pose.position.y,
tf::getYaw(latest_odom_msg_.pose.pose.orientation),
latest_odom_tf_ );

inc_tf = last_used_odom_tf_.inverse() * latest_odom_tf_;
ROS_DEBUG("increment_tf:[%f, %f, %f", inc_tf.getOrigin().x(),
inc_tf.getOrigin().y(),
inc_tf.getRotation().getAngle() );
last_used_odom_tf_ = latest_odom_tf_;
}

// **** use imu
if (use_imu_ && received_imu_)
{
pr_ch_a = tf::getYaw(latest_imu_msg_.orientation) -
tf::getYaw(last_used_imu_msg_.orientation);

if (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI;
else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI;

createTfFromXYTheta(0, 0, pr_ch_a, inc_tf );
last_used_imu_msg_ = latest_imu_msg_;
}

}

void LaserScanMatcher::createTfFromXYTheta(
Expand Down