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.

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

Cleanup of old local cloud filtering code #33

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open
Changes from all commits
Commits
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
Cleanup of old local cloud filtering code
dotPiano committed Dec 22, 2017
commit de786a1e9fd2cd3a1931584a73cce0c8dff1b231
31 changes: 0 additions & 31 deletions laser_slam_ros/include/laser_slam_ros/common.hpp
Original file line number Diff line number Diff line change
@@ -191,37 +191,6 @@ static void convert_to_point_cloud_2_msg(const PointCloudT& cloud,
converted->header.frame_id = frame;
}

static void applyCylindricalFilter(const PclPoint& center, double radius_m,
double height_m, bool remove_point_inside,
PointCloud* cloud) {
CHECK_NOTNULL(cloud);
PointCloud filtered_cloud;

const double radius_squared = pow(radius_m, 2.0);
const double height_halved_m = height_m / 2.0;

for (size_t i = 0u; i < cloud->size(); ++i) {
if (remove_point_inside) {
if ((pow(cloud->points[i].x - center.x, 2.0)
+ pow(cloud->points[i].y - center.y, 2.0)) >= radius_squared ||
abs(cloud->points[i].z - center.z) >= height_halved_m) {
filtered_cloud.points.push_back(cloud->points[i]);
}
} else {
if ((pow(cloud->points[i].x - center.x, 2.0)
+ pow(cloud->points[i].y - center.y, 2.0)) <= radius_squared &&
abs(cloud->points[i].z - center.z) <= height_halved_m) {
filtered_cloud.points.push_back(cloud->points[i]);
}
}
}

filtered_cloud.width = 1;
filtered_cloud.height = filtered_cloud.points.size();

*cloud = filtered_cloud;
}

} // namespace laser_slam_ros

#endif // LASER_SLAM_ROS_COMMON_HPP_
17 changes: 1 addition & 16 deletions laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp
Original file line number Diff line number Diff line change
@@ -33,32 +33,19 @@ class LaserSlamWorker {
void publishTrajectory(const laser_slam::Trajectory& trajectory,
const ros::Publisher& publisher) const;

/// \brief Publish the map.
void publishMap();

/// \brief Publish the estimated trajectory and the odometry only based trajectory.
void publishTrajectories();

void getLocalMapFiltered(laser_slam_ros::PointCloud* local_map_filtered);

// Get a filtered map and apply map separation if desired.
void getFilteredMap(laser_slam_ros::PointCloud* filtered_map);

// Get a vector containing the optimized point clouds recorded since
// the last call to this method. This call clears the point cloud queue.
std::vector<laser_slam_ros::PointCloud> getQueuedPoints();

void clearLocalMap();

tf::StampedTransform getWorldToOdom();

void getTrajectory(laser_slam::Trajectory* out_trajectory) const;

void getOdometryTrajectory(laser_slam::Trajectory* out_trajectory) const;

void updateLocalMap(const laser_slam::SE3& last_pose_before_update,
const laser_slam::Time last_pose_before_update_timestamp_ns);

/// \brief Computes the transform between a start pose and the pose evaluated at the specified
/// end timestamp.
/// \param start_pose The starting pose.
@@ -135,9 +122,7 @@ class LaserSlamWorker {
// Pointer to the incremental estimator.
std::shared_ptr<laser_slam::IncrementalEstimator> incremental_estimator_;

// Contains the map which is estimated by the sliding window.
// TODO(mattia): switch from local_map_ to local_map_queue_
laser_slam_ros::PointCloud local_map_;
// Queue of points that have been registered, but have not been processed yet.
std::vector<laser_slam_ros::PointCloud> local_map_queue_;

laser_slam_ros::PointCloud local_map_filtered_;
149 changes: 0 additions & 149 deletions laser_slam_ros/src/laser_slam_worker.cpp
Original file line number Diff line number Diff line change
@@ -236,11 +236,6 @@ void LaserSlamWorker::scanCallback(const sensor_msgs::PointCloud2& cloud_msg_in)
if (params_.create_filtered_map) {
if (new_fixed_cloud_pcl.size() > 0u) {
std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
if (local_map_.size() > 0u) {
local_map_ += new_fixed_cloud_pcl;
} else {
local_map_ = new_fixed_cloud_pcl;
}
local_map_queue_.push_back(new_fixed_cloud_pcl);
}
}
@@ -313,34 +308,6 @@ void LaserSlamWorker::publishTrajectory(const Trajectory& trajectory,
publisher.publish(traj_msg);
}

void LaserSlamWorker::publishMap() {
// TODO make thread safe.
if (local_map_.size() > 0) {
PointCloud filtered_map;
getFilteredMap(&filtered_map);

//maximumNumberPointsFilter(&filtered_map);
// if (params_.publish_full_map) {
// sensor_msgs::PointCloud2 msg;
// convert_to_point_cloud_2_msg(filtered_map, params_.world_frame, &msg);
// point_cloud_pub_.publish(msg);
// }
if (params_.publish_local_map) {
sensor_msgs::PointCloud2 msg;
{
std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
convert_to_point_cloud_2_msg(local_map_filtered_, params_.world_frame, &msg);
}
local_map_pub_.publish(msg);
}
// if (params_.publish_distant_map) {
// sensor_msgs::PointCloud2 msg;
// convert_to_point_cloud_2_msg(distant_map_, params_.world_frame, &msg);
// distant_map_pub_.publish(msg);
// }
}
}

void LaserSlamWorker::publishTrajectories() {
Trajectory trajectory;
laser_track_->getTrajectory(&trajectory);
@@ -383,102 +350,6 @@ std::vector<laser_slam_ros::PointCloud> LaserSlamWorker::getQueuedPoints() {
return new_points;
}

// TODO one shot of cleaning.
void LaserSlamWorker::getFilteredMap(PointCloud* filtered_map) {
laser_slam::Pose current_pose = laser_track_->getCurrentPose();

PclPoint current_position;
current_position.x = current_pose.T_w.getPosition()[0];
current_position.y = current_pose.T_w.getPosition()[1];
current_position.z = current_pose.T_w.getPosition()[2];

// Apply the cylindrical filter on the local map and get a copy.
PointCloud local_map;
{
std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
local_map = local_map_;
applyCylindricalFilter(current_position, params_.distance_to_consider_fixed,
40, false, &local_map_);
}

// Apply a voxel filter.
laser_slam::Clock clock;

PointCloudPtr local_map_ptr(new PointCloud());
pcl::copyPointCloud<PclPoint, PclPoint>(local_map, *local_map_ptr);

PointCloud local_map_filtered;

voxel_filter_.setInputCloud(local_map_ptr);
voxel_filter_.filter(local_map_filtered);

clock.takeTime();

if (params_.separate_distant_map) {
// If separating the map is enabled, the distance between each point in the local_map_ will
// be compared to the current robot position. Points which are far from the robot will
// be transfered to the distant_map_. This is helpful for publishing (points in distant_map_
// need to be filtered only once) and for any other processing which needs to be done only
// when a map is distant from robot and can be assumed as static (until loop closure).

// TODO(renaud) Is there a way to separate the cloud without having to transform in sensor
// frame by setting the position to compute distance from?
// Transform local_map_ in sensor frame.
clock.start();

// Save before removing points.
PointCloud new_distant_map = local_map_filtered;

applyCylindricalFilter(current_position, params_.distance_to_consider_fixed,
40, false, &local_map_filtered);

applyCylindricalFilter(current_position, params_.distance_to_consider_fixed,
40, true, &new_distant_map);

{
std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
local_map_filtered_ = local_map_filtered;
}

// Add the new_distant_map to the distant_map_.
// TODO add lock if used
if (distant_map_.size() > 0u) {
distant_map_ += new_distant_map;
} else {
distant_map_ = new_distant_map;
}

*filtered_map = local_map_filtered;
*filtered_map += distant_map_;

clock.takeTime();
// LOG(INFO) << "new_local_map.size() " << local_map.size();
// LOG(INFO) << "new_distant_map.size() " << new_distant_map.size();
// LOG(INFO) << "distant_map_.size() " << distant_map_.size();
// LOG(INFO) << "Separating done! Took " << clock.getRealTime() << " ms.";
} else {
*filtered_map = local_map;
}
}

void LaserSlamWorker::getLocalMapFiltered(PointCloud* local_map_filtered) {
CHECK_NOTNULL(local_map_filtered);
std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
*local_map_filtered = local_map_filtered_;
}

void LaserSlamWorker::clearLocalMap() {
{
std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
local_map_.clear();
}

{
std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
local_map_filtered_.clear();
}
}

tf::StampedTransform LaserSlamWorker::getWorldToOdom() {
std::lock_guard<std::recursive_mutex> lock_world_to_odom(world_to_odom_mutex_);
tf::StampedTransform world_to_odom = world_to_odom_;
@@ -493,26 +364,6 @@ void LaserSlamWorker::getOdometryTrajectory(Trajectory* out_trajectory) const {
laser_track_->getOdometryTrajectory(out_trajectory);
}

void LaserSlamWorker::updateLocalMap(const SE3& last_pose_before_update,
const laser_slam::Time last_pose_before_update_timestamp_ns) {

Trajectory new_trajectory;
laser_track_->getTrajectory(&new_trajectory);

SE3 new_last_pose = new_trajectory.at(last_pose_before_update_timestamp_ns);

const Eigen::Matrix4f transform_matrix = (new_last_pose * last_pose_before_update.inverse()).
getTransformationMatrix().cast<float>();
{
std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
pcl::transformPointCloud(local_map_, local_map_, transform_matrix);
}
{
std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
pcl::transformPointCloud(local_map_filtered_, local_map_filtered_, transform_matrix);
}
}

laser_slam::SE3 LaserSlamWorker::getTransformBetweenPoses(
const laser_slam::SE3& start_pose, const laser_slam::Time end_pose_timestamp_ns) const {
Trajectory new_trajectory;