i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Voxel layer from costmap_2d package takes into account robot z position

Browse Source
This commit is contained in:
Prescillia
2019-11-05 15:37:49 -05:00
parent 3282b7b946
commit 420d7403dd
4 changed files with 701 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/costmap_2d/voxel_layer.cpp
+489
View File
@@ -0,0 +1,489 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "voxel_layer.h"
#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <boost/thread.hpp>
#include <pcl_conversions/pcl_conversions.h>
#define VOXEL_BITS 16
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::VoxelLayer, costmap_2d::Layer)
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using costmap_2d::Costmap2D;
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;
using costmap_2d::VoxelGrid;
namespace rtabmap_ros
{
void VoxelLayer::onInitialize()
{
ObstacleLayer::onInitialize();
ros::NodeHandle private_nh("~/" + name_);
std::string costmap_name_ = name_.substr(0, name_.find("/"));
ros::NodeHandle pnh("~/" + costmap_name_);
private_nh.param("publish_voxel_map", publish_voxel_, false);
pnh.param("robot_frame", robot_base_frame_, std::string("base_link"));
if (publish_voxel_)
voxel_pub_ = private_nh.advertise <VoxelGrid > ("voxel_grid", 1);
clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
}
void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
{
voxel_dsrv_ = new dynamic_reconfigure::Server<VoxelPluginConfig>(nh);
dynamic_reconfigure::Server<VoxelPluginConfig>::CallbackType cb = boost::bind(
&VoxelLayer::reconfigureCB, this, _1, _2);
voxel_dsrv_->setCallback(cb);
}
VoxelLayer::~VoxelLayer()
{
if (voxel_dsrv_)
delete voxel_dsrv_;
}
void VoxelLayer::reconfigureCB(VoxelPluginConfig &config, uint32_t level)
{
enabled_ = config.enabled;
footprint_clearing_enabled_ = config.footprint_clearing_enabled;
max_obstacle_height_ = config.max_obstacle_height;
size_z_ = config.z_voxels;
origin_z_ = config.origin_z;
z_resolution_ = config.z_resolution;
unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
mark_threshold_ = config.mark_threshold;
combination_method_ = config.combination_method;
matchSize();
}
void VoxelLayer::matchSize()
{
ObstacleLayer::matchSize();
voxel_grid_.resize(size_x_, size_y_, size_z_);
// ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_ && voxel_grid_.sizeZ() == size_z_);
}
void VoxelLayer::reset()
{
deactivate();
resetMaps();
voxel_grid_.reset();
activate();
}
void VoxelLayer::resetMaps()
{
Costmap2D::resetMaps();
voxel_grid_.reset();
}
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (rolling_window_) {
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2 );
}
if (!enabled_)
return;
useExtraBounds(min_x, min_y, max_x, max_y);
bool current = true;
std::vector<Observation> observations, clearing_observations;
// get the marking observations
current = getMarkingObservations(observations) && current;
// get the clearing observations
current = getClearingObservations(clearing_observations) && current;
// update the global current status
current_ = current;
// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}
// place the new obstacles into a priority queue... each with a priority of zero to begin with
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const Observation& obs = *it;
#ifdef COSTMAP_2D_POINTCLOUD2
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
// Note that these points are already filtered with max and min obstacle heights
// You will have to adjust param file to see points beyond default settings
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
const double x = *iter_x;
const double y = *iter_y;
const double z = *iter_z;
#else
pcl::PointCloud<pcl::PointXYZ>::const_iterator point_it = obs.cloud_->begin();
for (;point_it < obs.cloud_->end(); ++point_it) {
const double x = point_it->x;
const double y = point_it->y;
const double z = point_it->z;
#endif
// now we need to compute the map coordinates for the observation
unsigned int mx, my, mz;
if (!worldToMap3D(x, y, z, mx, my, mz))
continue;
if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
{
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(x, y, min_x, min_y, max_x, max_y);
}
}
}
if (publish_voxel_)
{
VoxelGrid grid_msg;
unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
grid_msg.size_x = voxel_grid_.sizeX();
grid_msg.size_y = voxel_grid_.sizeY();
grid_msg.size_z = voxel_grid_.sizeZ();
grid_msg.data.resize(size);
memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
grid_msg.origin.x = origin_x_;
grid_msg.origin.y = origin_y_;
grid_msg.origin.z = origin_z_;
grid_msg.resolutions.x = resolution_;
grid_msg.resolutions.y = resolution_;
grid_msg.resolutions.z = z_resolution_;
grid_msg.header.frame_id = global_frame_;
grid_msg.header.stamp = ros::Time::now();
voxel_pub_.publish(grid_msg);
}
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
{
// get the cell coordinates of the center point of the window
unsigned int mx, my;
if (!worldToMap(wx, wy, mx, my))
return;
// compute the bounds of the window
double start_x = wx - w_size_x / 2;
double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
// scale the window based on the bounds of the costmap
start_x = std::max(origin_x_, start_x);
start_y = std::max(origin_y_, start_y);
end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
// get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
// check for legality just in case
if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
return;
// we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &costmap_[index];
for (unsigned int j = map_sy; j <= map_ey; ++j)
{
for (unsigned int i = map_sx; i <= map_ex; ++i)
{
// if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
if (*current != LETHAL_OBSTACLE)
{
if (clear_no_info || *current != NO_INFORMATION)
{
*current = FREE_SPACE;
voxel_grid_.clearVoxelColumn(index);
}
}
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y)
{
size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
if (clearing_observation_cloud_size == 0)
return;
double sensor_x, sensor_y, sensor_z;
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
double oz = clearing_observation.origin_.z;
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
{
ROS_WARN_THROTTLE(
1.0,
"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
ox, oy, oz);
return;
}
bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
if (publish_clearing_points)
{
clearing_endpoints_.points.clear();
clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
}
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
double map_end_z = origin_z_ + size_z_ * z_resolution_;
#ifdef COSTMAP_2D_POINTCLOUD2
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
double wpx = *iter_x;
double wpy = *iter_y;
double wpz = *iter_z;
#else
pcl::PointCloud<pcl::PointXYZ>::const_iterator point_it = clearing_observation.cloud_->begin();
for (;point_it < clearing_observation.cloud_->end(); ++point_it) {
double wpx = point_it->x;
double wpy = point_it->y;
double wpz = point_it->z;
#endif
double distance = dist(ox, oy, oz, wpx, wpy, wpz);
double scaling_fact = 1.0, scaling_fact_z = 1.0;
scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
scaling_fact_z = std::max(std::min(scaling_fact_z, (distance - 2 * z_resolution_) / distance), 0.0);
wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact_z * (wpz - oz) + oz;
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = 1.0;
// the minimum value to raytrace from is the origin
if (wpz < origin_z_)
{
t = std::min(t, (origin_z_ - oz) / c);
}
if (wpx < origin_x_)
{
t = std::min(t, (origin_x_ - ox) / a);
}
if (wpy < origin_y_)
{
t = std::min(t, (origin_y_ - oy) / b);
}
// the maximum value to raytrace to is the end of the map
if (wpx > map_end_x)
{
t = std::min(t, (map_end_x - ox) / a);
}
if (wpy > map_end_y)
{
t = std::min(t, (map_end_y - oy) / b);
}
if (wpz > map_end_z)
{
t = std::min(t, (map_end_z - oz) / c);
}
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
double point_x, point_y, point_z;
if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
{
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cell_raytrace_range);
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
if (publish_clearing_points)
{
geometry_msgs::Point32 point;
point.x = wpx;
point.y = wpy;
point.z = wpz;
clearing_endpoints_.points.push_back(point);
}
}
}
if (publish_clearing_points)
{
clearing_endpoints_.header.frame_id = global_frame_;
#ifdef COSTMAP_2D_POINTCLOUD2
clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
#else
clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header.stamp);
#endif
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
clearing_endpoints_pub_.publish(clearing_endpoints_);
}
}
void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
{
int cell_oz;
// get the global pose of the robot
try
{
geometry_msgs::TransformStamped transformStamped;
#ifdef COSTMAP_2D_POINTCLOUD2
transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0));
#else
tf::StampedTransform stampedTransform;
tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0), stampedTransform);
tf::transformStampedTFToMsg(stampedTransform, transformStamped);
#endif
const double robot_z = transformStamped.transform.translation.z;
const double z_grid_height = z_resolution_ * size_z_;
const double new_origin_z = robot_z - z_grid_height / 2;
cell_oz = int((new_origin_z - origin_z_) / z_resolution_);
}
#ifdef COSTMAP_2D_POINTCLOUD2
catch (tf2::TransformException& ex)
#else
catch(tf::TransformException& ex)
#endif
{
ROS_ERROR("%s", ex.what());
// If the robot pose is not detected, the origin_z_ will remain the same.
cell_oz = 0;
}
// project the new origin into the grid
int cell_ox, cell_oy;
cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
// compute the associated world coordinates for the origin cell
// because we want to keep things grid-aligned
double new_grid_ox, new_grid_oy, new_grid_oz;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
new_grid_oz = origin_z_ + cell_oz * z_resolution_;
// to avoid casting from unsigned int to int a bunch of times
int size_x = size_x_;
int size_y = size_y_;
// we need to compute the overlap of the new and existing windows
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
lower_left_x = std::min(std::max(cell_ox, 0), size_x);
lower_left_y = std::min(std::max(cell_oy, 0), size_y);
upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// we need a map to store the obstacles in the window temporarily
//unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
unsigned int* voxel_map = voxel_grid_.getData();
// copy the local window in the costmap to the local map
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
cell_size_y);
// we'll reset our maps to unknown space if appropriate
resetMaps();
// update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy;
origin_z_ = new_grid_oz;
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion3D(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y, cell_oz);
// make sure to clean up
delete[] local_voxel_map;
}
} // namespace rtabmap_ros
src/costmap_2d/voxel_layer.h
+204
View File
@@ -0,0 +1,204 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef RTABMAP_ROS_VOXEL_LAYER_H_
#define RTABMAP_ROS_VOXEL_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/VoxelGrid.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <message_filters/subscriber.h>
#include <dynamic_reconfigure/server.h>
#include <costmap_2d/VoxelPluginConfig.h>
#include <costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h>
using costmap_2d::VoxelPluginConfig;
namespace rtabmap_ros
{
class VoxelLayer : public costmap_2d::ObstacleLayer
{
public:
VoxelLayer() :
voxel_grid_(0, 0, 0)
{
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
}
virtual ~VoxelLayer();
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
void updateOrigin(double new_origin_x, double new_origin_y);
bool isDiscretized()
{
return true;
}
virtual void matchSize();
virtual void reset();
protected:
virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
virtual void resetMaps();
private:
void reconfigureCB(VoxelPluginConfig &config, uint32_t level);
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
dynamic_reconfigure::Server<VoxelPluginConfig> *voxel_dsrv_;
bool publish_voxel_;
std::string robot_base_frame_;
ros::Publisher voxel_pub_;
voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;
ros::Publisher clearing_endpoints_pub_;
sensor_msgs::PointCloud clearing_endpoints_;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
{
if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
return false;
mx = ((wx - origin_x_) / resolution_);
my = ((wy - origin_y_) / resolution_);
mz = ((wz - origin_z_) / z_resolution_);
if (mx < size_x_ && my < size_y_ && mz < size_z_)
return true;
return false;
}
inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz)
{
if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
return false;
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
mz = (int)((wz - origin_z_) / z_resolution_);
if (mx < size_x_ && my < size_y_ && mz < size_z_)
return true;
return false;
}
inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz)
{
// returns the center point of the cell
wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
wz = origin_z_ + (mz + 0.5) * z_resolution_;
}
inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
{
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
}
/**
* @brief Copy a region of a source map into a destination map
* @param source_map The source map
* @param sm_lower_left_x The lower left x point of the source map to start the copy
* @param sm_lower_left_y The lower left y point of the source map to start the copy
* @param sm_size_x The x size of the source map
* @param dest_map The destination map
* @param dm_lower_left_x The lower left x point of the destination map to start the copy
* @param dm_lower_left_y The lower left y point of the destination map to start the copy
* @param dm_size_x The x size of the destination map
* @param region_size_x The x size of the region to copy
* @param region_size_y The y size of the region to copy
*/
template<typename data_type>
void copyMapRegion3D(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
unsigned int region_size_y, int z_shift)
{
// we'll first need to compute the starting points for each map
// this is like getting voxel column. We are not taking into account the z position of the voxel
data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
uint32_t marked_bits_mask = (data_type) 0xFFFF0000;
uint32_t unknown_bits_mask = (data_type) 0x0000FFFF;
// now, we'll copy the source map into the destination map
for (unsigned int i = 0; i < region_size_y; ++i)
{
memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
for (unsigned int j = 0; j < dm_size_x; j++) {
if (z_shift > 0) {
dm_index[j] = ((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) |
((~((data_type) 0) << sizeof(data_type) * 4 - z_shift) |
(dm_index[j] & unknown_bits_mask) >> z_shift) & unknown_bits_mask;
} else if (z_shift < 0) {
dm_index[j] = (dm_index[j] & marked_bits_mask) << z_shift * -1 |
(dm_index[j] << z_shift * -1 & unknown_bits_mask |
~(~((unsigned int) 0) << z_shift * -1));
}
}
dm_index += dm_size_x;
sm_index += sm_size_x;
}
}
};
} // namespace rtabmap_ros
#endif // RTABMAP_ROS_VOXEL_LAYER_H_