Grid based representation of planar region. More...
#include <grid_plane.h>
Public Types | |
typedef boost::tuple< int, int > | IndexPair |
typedef std::set< IndexPair > | IndexPairSet |
typedef boost::shared_ptr < GridPlane > | Ptr |
Public Member Functions | |
virtual void | addIndexPair (IndexPair pair) |
Add IndexPair to this instance. | |
virtual GridPlane::Ptr | clone () |
virtual GridPlane::Ptr | dilate (int num) |
Dilate grid cells with specified number of pixels. | |
virtual GridPlane::Ptr | erode (int num) |
Erode grid cells with specified number of pixels. | |
virtual void | fillCellsFromCube (Cube &cube) |
virtual size_t | fillCellsFromPointCloud (pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, double distance_threshold) |
virtual size_t | fillCellsFromPointCloud (pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, double distance_threshold, std::set< int > &non_plane_indices) |
virtual size_t | fillCellsFromPointCloud (pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, double distance_threshold, double normal_threshold, std::set< int > &non_plane_indices) |
virtual ConvexPolygon::Ptr | getPolygon () |
return ConvexPolygon pointer of this instance. | |
virtual double | getResolution () |
GridPlane (ConvexPolygon::Ptr plane, const double resolution) | |
virtual bool | isOccupied (const IndexPair &pair) |
virtual bool | isOccupied (const Eigen::Vector3f &p) |
p should be local coordinate | |
virtual bool | isOccupiedGlobal (const Eigen::Vector3f &p) |
p should be global coordinate | |
virtual IndexPair | projectLocalPointAsIndexPair (const Eigen::Vector3f &p) |
Project 3-D point to GridPlane::IndexPair. p should be represented in local coordinates. | |
virtual jsk_recognition_msgs::SimpleOccupancyGrid | toROSMsg () |
virtual Eigen::Vector3f | unprojectIndexPairAsGlobalPoint (const IndexPair &pair) |
Unproject GridPlane::IndexPair to 3-D global point. | |
virtual Eigen::Vector3f | unprojectIndexPairAsLocalPoint (const IndexPair &pair) |
Unproject GridPlane::IndexPair to 3-D local point. | |
virtual | ~GridPlane () |
Static Public Member Functions | |
static GridPlane | fromROSMsg (const jsk_recognition_msgs::SimpleOccupancyGrid &rosmsg, const Eigen::Affine3f &offset) |
Construct GridPlane object from jsk_recognition_msgs::SimpleOccupancyGrid. | |
Protected Attributes | |
IndexPairSet | cells_ |
ConvexPolygon::Ptr | convex_ |
double | resolution_ |
Grid based representation of planar region.
Each cell represents a square region as belows: +--------+ | | | + | | | +--------+
The width and height of the cell is equivalent to resolution_, and the value of cells_ represents a center point. (i, j) means rectanglar region of (x, y) which satisfies followings: i * resolution - 0.5 * resolution <= x < i * resolution + 0.5 * resolution j * resolution - 0.5 * resolution <= y < j * resolution + 0.5 * resolution
Definition at line 73 of file grid_plane.h.
typedef boost::tuple<int, int> jsk_recognition_utils::GridPlane::IndexPair |
Definition at line 77 of file grid_plane.h.
Definition at line 78 of file grid_plane.h.
typedef boost::shared_ptr<GridPlane> jsk_recognition_utils::GridPlane::Ptr |
Definition at line 76 of file grid_plane.h.
jsk_recognition_utils::GridPlane::GridPlane | ( | ConvexPolygon::Ptr | plane, |
const double | resolution | ||
) |
Definition at line 43 of file grid_plane.cpp.
jsk_recognition_utils::GridPlane::~GridPlane | ( | ) | [virtual] |
Definition at line 49 of file grid_plane.cpp.
void jsk_recognition_utils::GridPlane::addIndexPair | ( | IndexPair | pair | ) | [virtual] |
Add IndexPair to this instance.
Definition at line 65 of file grid_plane.cpp.
GridPlane::Ptr jsk_recognition_utils::GridPlane::clone | ( | ) | [virtual] |
Definition at line 144 of file grid_plane.cpp.
GridPlane::Ptr jsk_recognition_utils::GridPlane::dilate | ( | int | num | ) | [virtual] |
Dilate grid cells with specified number of pixels.
Definition at line 70 of file grid_plane.cpp.
GridPlane::Ptr jsk_recognition_utils::GridPlane::erode | ( | int | num | ) | [virtual] |
Erode grid cells with specified number of pixels.
Definition at line 91 of file grid_plane.cpp.
void jsk_recognition_utils::GridPlane::fillCellsFromCube | ( | Cube & | cube | ) | [virtual] |
Definition at line 166 of file grid_plane.cpp.
size_t jsk_recognition_utils::GridPlane::fillCellsFromPointCloud | ( | pcl::PointCloud< pcl::PointNormal >::Ptr & | cloud, |
double | distance_threshold | ||
) | [virtual] |
Definition at line 288 of file grid_plane.cpp.
size_t jsk_recognition_utils::GridPlane::fillCellsFromPointCloud | ( | pcl::PointCloud< pcl::PointNormal >::Ptr & | cloud, |
double | distance_threshold, | ||
std::set< int > & | non_plane_indices | ||
) | [virtual] |
Definition at line 218 of file grid_plane.cpp.
size_t jsk_recognition_utils::GridPlane::fillCellsFromPointCloud | ( | pcl::PointCloud< pcl::PointNormal >::Ptr & | cloud, |
double | distance_threshold, | ||
double | normal_threshold, | ||
std::set< int > & | non_plane_indices | ||
) | [virtual] |
Definition at line 227 of file grid_plane.cpp.
GridPlane jsk_recognition_utils::GridPlane::fromROSMsg | ( | const jsk_recognition_msgs::SimpleOccupancyGrid & | rosmsg, |
const Eigen::Affine3f & | offset = Eigen::Affine3f::Identity() |
||
) | [static] |
Construct GridPlane object from jsk_recognition_msgs::SimpleOccupancyGrid.
Definition at line 320 of file grid_plane.cpp.
virtual ConvexPolygon::Ptr jsk_recognition_utils::GridPlane::getPolygon | ( | ) | [inline, virtual] |
return ConvexPolygon pointer of this instance.
Definition at line 154 of file grid_plane.h.
virtual double jsk_recognition_utils::GridPlane::getResolution | ( | ) | [inline, virtual] |
Definition at line 95 of file grid_plane.h.
bool jsk_recognition_utils::GridPlane::isOccupied | ( | const IndexPair & | pair | ) | [virtual] |
Definition at line 118 of file grid_plane.cpp.
bool jsk_recognition_utils::GridPlane::isOccupied | ( | const Eigen::Vector3f & | p | ) | [virtual] |
p should be local coordinate
Definition at line 133 of file grid_plane.cpp.
bool jsk_recognition_utils::GridPlane::isOccupiedGlobal | ( | const Eigen::Vector3f & | p | ) | [virtual] |
p should be global coordinate
Definition at line 139 of file grid_plane.cpp.
GridPlane::IndexPair jsk_recognition_utils::GridPlane::projectLocalPointAsIndexPair | ( | const Eigen::Vector3f & | p | ) | [virtual] |
Project 3-D point to GridPlane::IndexPair. p should be represented in local coordinates.
Definition at line 54 of file grid_plane.cpp.
jsk_recognition_msgs::SimpleOccupancyGrid jsk_recognition_utils::GridPlane::toROSMsg | ( | ) | [virtual] |
Definition at line 296 of file grid_plane.cpp.
Eigen::Vector3f jsk_recognition_utils::GridPlane::unprojectIndexPairAsGlobalPoint | ( | const IndexPair & | pair | ) | [virtual] |
Unproject GridPlane::IndexPair to 3-D global point.
Definition at line 159 of file grid_plane.cpp.
Eigen::Vector3f jsk_recognition_utils::GridPlane::unprojectIndexPairAsLocalPoint | ( | const IndexPair & | pair | ) | [virtual] |
Unproject GridPlane::IndexPair to 3-D local point.
Definition at line 151 of file grid_plane.cpp.
IndexPairSet jsk_recognition_utils::GridPlane::cells_ [protected] |
Definition at line 163 of file grid_plane.h.
Definition at line 162 of file grid_plane.h.
double jsk_recognition_utils::GridPlane::resolution_ [protected] |
Definition at line 164 of file grid_plane.h.