Program Listing for File spatio_temporal_voxel_grid.hpp
↰ Return to documentation for file (include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
)
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2018, Simbe Robotics, 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 Simbe Robotics, 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: Steve Macenski (steven.macenski@simberobotics.com)
* Purpose: Implement OpenVDB's voxel library with ray tracing for our
* internal voxel grid layer.
*********************************************************************/
#ifndef SPATIO_TEMPORAL_VOXEL_LAYER__SPATIO_TEMPORAL_VOXEL_GRID_HPP_
#define SPATIO_TEMPORAL_VOXEL_LAYER__SPATIO_TEMPORAL_VOXEL_GRID_HPP_
// STL
#include <math.h>
#include <unordered_map>
#include <unordered_set>
#include <ctime>
#include <iostream>
#include <utility>
#include <vector>
#include <memory>
#include <string>
// PCL
#include "pcl/common/transforms.h"
#include "pcl/PCLPointCloud2.h"
// ROS
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
// msgs
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/point32.hpp"
// OpenVDB
#include "openvdb/openvdb.h"
#include "openvdb/tools/GridTransformer.h"
#include "openvdb/tools/RayIntersector.h"
// measurement struct and buffer
#include "spatio_temporal_voxel_layer/measurement_buffer.hpp"
#include "spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp"
#include "spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp"
// Mutex and locks
#include "boost/thread.hpp"
#include "boost/thread/recursive_mutex.hpp"
namespace volume_grid
{
enum GlobalDecayModel
{
LINEAR = 0,
EXPONENTIAL = 1,
PERSISTENT = 2
};
// Structure for an occupied cell for map
struct occupany_cell
{
occupany_cell(const double & _x, const double & _y)
: x(_x), y(_y)
{
}
bool operator==(const occupany_cell & other) const
{
return x == other.x && y == other.y;
}
double x, y;
};
// Structure for wrapping frustum model and necessary metadata
struct frustum_model
{
frustum_model(geometry::Frustum * _frustum, const double & _factor)
: frustum(_frustum), accel_factor(_factor)
{
}
~frustum_model()
{
if (frustum) {
delete frustum;
}
}
geometry::Frustum * frustum;
const double accel_factor;
};
// Core voxel grid structure and interface
class SpatioTemporalVoxelGrid
{
public:
// conveniences for line lengths
typedef openvdb::math::Ray<openvdb::Real> GridRay;
typedef openvdb::math::Ray<openvdb::Real>::Vec3T Vec3Type;
SpatioTemporalVoxelGrid(
rclcpp::Clock::SharedPtr clock,
const float & voxel_size, const double & background_value,
const int & decay_model, const double & voxel_decay,
const bool & pub_voxels);
~SpatioTemporalVoxelGrid(void);
// Core making and clearing functions
void Mark(const std::vector<observation::MeasurementReading> & marking_observations);
void operator()(const observation::MeasurementReading & obs) const;
void ClearFrustums(
const std::vector<observation::MeasurementReading> & clearing_observations,
std::unordered_set<occupany_cell> & cleared_cells);
// Get the pointcloud of the underlying occupancy grid
void GetOccupancyPointCloud(std::unique_ptr<sensor_msgs::msg::PointCloud2> & pc2);
std::unordered_map<occupany_cell, uint> * GetFlattenedCostmap();
// Clear the grid
bool ResetGrid(void);
void ResetGridArea(const occupany_cell & start, const occupany_cell & end, bool invert_area=false);
// Save the file to file with size information
bool SaveGrid(const std::string & file_name, double & map_size_bytes);
protected:
// Initialize grid metadata and library
void InitializeGrid(void);
// grid accessor methods
bool MarkGridPoint(const openvdb::Coord & pt, const double & value) const;
bool ClearGridPoint(const openvdb::Coord & pt) const;
// Check occupancy status of the grid
bool IsGridEmpty(void) const;
// Get time information for clearing
double GetTemporalClearingDuration(const double & time_delta);
double GetFrustumAcceleration(
const double & time_delta, const double & acceleration_factor);
void TemporalClearAndGenerateCostmap(
std::vector<frustum_model> & frustums,
std::unordered_set<occupany_cell> & cleared_cells);
// Populate the costmap ROS api and pointcloud with a marked point
void PopulateCostmapAndPointcloud(const openvdb::Coord & pt);
// Utilities for tranformation
openvdb::Vec3d WorldToIndex(const openvdb::Vec3d & coord) const;
openvdb::Vec3d IndexToWorld(const openvdb::Coord & coord) const;
rclcpp::Clock::SharedPtr _clock;
mutable openvdb::DoubleGrid::Ptr _grid;
int _decay_model;
double _background_value, _voxel_size, _voxel_decay;
bool _pub_voxels;
std::unique_ptr<std::vector<geometry_msgs::msg::Point32>> _grid_points;
std::unordered_map<occupany_cell, uint> * _cost_map;
boost::mutex _grid_lock;
};
} // namespace volume_grid
// hash function for unordered_map of occupancy_cells
namespace std
{
template<>
struct hash<volume_grid::occupany_cell>
{
std::size_t operator()(const volume_grid::occupany_cell & k) const
{
return (std::hash<double>()(k.x) ^ (std::hash<double>()(k.y) << 1)) >> 1;
}
};
} // namespace std
#endif // SPATIO_TEMPORAL_VOXEL_LAYER__SPATIO_TEMPORAL_VOXEL_GRID_HPP_