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

Odometry noiser #43

Open
wants to merge 3 commits into
base: feature/color_segmentation
Choose a base branch
from
Open

Odometry noiser #43

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
70e7c18
Add noise to odometry, if desired.
Nov 2, 2020
2491494
Merge branch 'feature/color_segmentation' into feature/odometry_noiser
smauq Nov 13, 2020
718e5f2
Integrated drift module better into segmap for evaluation
smauq Nov 22, 2020
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
19 changes: 19 additions & 0 deletions laser_slam_ros/include/laser_slam_ros/common.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,11 @@ struct LaserSlamWorkerParams {
bool publish_full_map;
bool publish_distant_map;
double map_publication_rate_hz;

// Odometry noiser
bool enable_drift;
float noise_x_mean, noise_y_mean, noise_z_mean, noise_yaw_mean, noise_att_mean;
float noise_x_stddev, noise_y_stddev, noise_z_stddev, noise_yaw_stddev, noise_att_stddev;
}; // struct LaserSlamWorkerParams

static LaserSlamWorkerParams getLaserSlamWorkerParams(const ros::NodeHandle& nh,
Expand Down Expand Up @@ -89,6 +94,20 @@ static LaserSlamWorkerParams getLaserSlamWorkerParams(const ros::NodeHandle& nh,
nh.getParam(ns + "/distant_map_pub_topic", params.distant_map_pub_topic);
nh.getParam(ns + "/get_laser_track_srv_topic", params.get_laser_track_srv_topic);

//#####ODOMETRY NOISER###############
nh.param(ns + "/enable_drift", params.enable_drift, false);
nh.getParam(ns + "/noise_x_mean", params.noise_x_mean);
nh.getParam(ns + "/noise_y_mean", params.noise_y_mean);
nh.getParam(ns + "/noise_z_mean", params.noise_z_mean);
nh.getParam(ns + "/noise_yaw_mean", params.noise_yaw_mean);
nh.getParam(ns + "/noise_att_mean", params.noise_att_mean);
nh.getParam(ns + "/noise_x_stddev", params.noise_x_stddev);
nh.getParam(ns + "/noise_y_stddev", params.noise_y_stddev);
nh.getParam(ns + "/noise_z_stddev", params.noise_z_stddev);
nh.getParam(ns + "/noise_yaw_stddev", params.noise_yaw_stddev);
nh.getParam(ns + "/noise_att_stddev", params.noise_att_stddev);
//###################################

return params;
}

Expand Down
25 changes: 24 additions & 1 deletion laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,13 @@
#include <sensor_msgs/PointCloud2.h>
#include <std_srvs/Empty.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include "laser_slam_ros/GetLaserTrackSrv.h"
#include "laser_slam_ros/common.hpp"

#include <random>

namespace laser_slam_ros {

class LaserSlamWorker {
Expand Down Expand Up @@ -75,7 +78,15 @@ class LaserSlamWorker {

bool exportTrajectoryServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);

bool exportDriftServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);

laser_slam::SE3 getGrountTruthPose() {
return tfTransformToSE3(T_W_BLast_);
}

private:
// Convert a tf::Transform to a laser_slam::SE3.
laser_slam::SE3 tfTransformToSE3(const tf::Transform& tf_transform);
// Convert a tf::StampedTransform to a laser_slam::Pose.
laser_slam::Pose tfTransformToPose(const tf::StampedTransform& tf_transform);
// TODO: common.hpp?
Expand All @@ -97,7 +108,18 @@ class LaserSlamWorker {
bool getLaserTracksServiceCall(laser_slam_ros::GetLaserTrackSrv::Request& request,
laser_slam_ros::GetLaserTrackSrv::Response& response);

private:
// Odometry noiser
bool enable_drift_ = false;
tf::Transform T_W_BLast_;
tf::Transform T_W_BdLast_;

std::default_random_engine generator_;
std::normal_distribution<float> dist_x_;
std::normal_distribution<float> dist_y_;
std::normal_distribution<float> dist_z_;
std::normal_distribution<float> dist_yaw_;
std::normal_distribution<float> dist_att_;

LaserSlamWorkerParams params_;

unsigned int worker_id_;
Expand Down Expand Up @@ -127,6 +149,7 @@ class LaserSlamWorker {
// Services.
ros::ServiceServer get_laser_track_srv_;
ros::ServiceServer export_trajectory_srv_;
ros::ServiceServer export_drift_srv_;

tf::TransformListener *tf_listener_;

Expand Down
69 changes: 67 additions & 2 deletions laser_slam_ros/src/laser_slam_worker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,23 @@ void LaserSlamWorker::init(
trajectory_pub_ = nh.advertise<nav_msgs::Path>(params_.trajectory_pub_topic,
kPublisherQueueSize, true);

// Odometry noiser
enable_drift_ = params.enable_drift;
if (enable_drift_) {
T_W_BdLast_.setIdentity();
T_W_BLast_.setIdentity();
dist_x_.param(std::normal_distribution<float>::param_type(
params_.noise_x_mean, params_.noise_x_stddev));
dist_y_.param(std::normal_distribution<float>::param_type(
params_.noise_y_mean, params_.noise_y_stddev));
dist_z_.param(std::normal_distribution<float>::param_type(
params_.noise_z_mean, params_.noise_z_stddev));
dist_yaw_.param(std::normal_distribution<float>::param_type(
params_.noise_yaw_mean, params_.noise_yaw_stddev));
dist_att_.param(std::normal_distribution<float>::param_type(
params_.noise_att_mean, params_.noise_att_stddev));
}

if (params_.publish_local_map) {
local_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>(params_.local_map_pub_topic,
kPublisherQueueSize);
Expand Down Expand Up @@ -96,7 +113,46 @@ void LaserSlamWorker::scanCallback(const sensor_msgs::PointCloud2& cloud_msg_in)
// Get the tf transform.
tf::StampedTransform tf_transform;
tf_listener_->lookupTransform(params_.odom_frame, params_.sensor_frame,
cloud_msg_in.header.stamp, tf_transform);
cloud_msg_in.header.stamp, tf_transform);

// Add drift
// B = Sensor frame. Bd = Drifted sensor frame. W = Odom frame.
if(enable_drift_) {
tf::Transform T_W_B = tf_transform;

// Compute odometry since last update T_BLast_B.
tf::Transform T_BLast_B = (T_W_BLast_.inverse()) * T_W_B;
tf::Vector3 transl_gt = T_BLast_B.getOrigin();
tf::Quaternion quat_gt = T_BLast_B.getRotation();

// Add noise in local frame. T_BdLast_Bd = adddNoise(T_BLast_B)
float noise_x = dist_x_(generator_);
float noise_y = dist_y_(generator_);
float noise_z = dist_z_(generator_);
float noise_yaw = dist_yaw_(generator_);
float noise_roll = dist_att_(generator_);
float noise_pitch = dist_att_(generator_);
tf::Quaternion quat_noise;
quat_noise.setRPY(noise_roll, noise_pitch, noise_yaw);

transl_gt.setX(noise_x + T_BLast_B.getOrigin().x());
transl_gt.setY(noise_y + T_BLast_B.getOrigin().y());
transl_gt.setZ(noise_z + T_BLast_B.getOrigin().z());
tf::Transform T_BdLast_Bd = T_BLast_B;
T_BdLast_Bd.setOrigin(transl_gt); // Add translation drift.
T_BdLast_Bd.setRotation(quat_gt * quat_noise); // Add rotation drift.

// Compute drifted sensor frame T_W_Bd.
tf::Transform T_W_Bd = T_W_BdLast_ * T_BdLast_Bd;
tf_transform.setData(T_W_Bd);

// Store values for next iteration.
T_W_BdLast_ = T_W_Bd;
T_W_BLast_ = T_W_B;
} else {
T_W_BLast_ = tf_transform;
T_W_BdLast_ = tf_transform;
}

bool process_scan = false;
SE3 current_pose;
Expand Down Expand Up @@ -348,7 +404,16 @@ void LaserSlamWorker::publishTrajectories() {
if (trajectory.size() > 0u) publishTrajectory(trajectory, trajectory_pub_);
}

// TODO can we move?
SE3 LaserSlamWorker::tfTransformToSE3(const tf::Transform& tf_transform) {
SE3::Position pos(tf_transform.getOrigin().getX(), tf_transform.getOrigin().getY(),
tf_transform.getOrigin().getZ());
SE3::Rotation::Implementation rot(tf_transform.getRotation().getW(),
tf_transform.getRotation().getX(),
tf_transform.getRotation().getY(),
tf_transform.getRotation().getZ());
return SE3(pos, rot);
}

Pose LaserSlamWorker::tfTransformToPose(const tf::StampedTransform& tf_transform) {
// Register new pose.
Pose pose;
Expand Down