#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <string>
#include <vector>
#include <ostream>
#include "ros/serialization.h"
#include "ros/builtin_message_traits.h"
#include "ros/message_operations.h"
#include "ros/message.h"
#include "ros/time.h"
#include <Eigen/Core>
#include <cfloat>
Go to the source code of this file.
Classes | |
struct | cloud_geometry::Leaf |
Simple leaf (3d box) structure) holding a centroid and the number of points in the leaf. More... | |
Namespaces | |
namespace | cloud_geometry |
Functions | |
void | cloud_geometry::cerr_p (const std::vector< double > &p) |
void | cloud_geometry::cerr_p (const geometry_msgs::Point32 &p) |
Write the point data to screen (stderr). | |
void | cloud_geometry::cerr_poly (const geometry_msgs::Polygon &poly) |
Write the polygon data to screen (stderr). | |
bool | cloud_geometry::checkPointEqual (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2, double eps=1e-10) |
Check whether two given points are equal with some epsilon delta. | |
void | cloud_geometry::cross (const std::vector< double > &p1, const std::vector< double > &p2, std::vector< double > &p3) |
Compute the cross product between two points (vectors). | |
geometry_msgs::Point32 | cloud_geometry::cross (const geometry_msgs::Point32 &p1, const std::vector< double > &p2) |
Compute the cross product between two points (vectors). | |
geometry_msgs::Point32 | cloud_geometry::cross (const std::vector< double > &p1, const geometry_msgs::Point32 &p2) |
Compute the cross product between two points (vectors). | |
geometry_msgs::Point32 | cloud_geometry::cross (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2) |
Compute the cross product between two points (vectors). | |
double | cloud_geometry::dot (const std::vector< double > &p1, const std::vector< double > &p2) |
Compute the dot product between two points (vectors). | |
double | cloud_geometry::dot (const geometry_msgs::Vector3 &p1, const geometry_msgs::Vector3 &p2) |
Compute the dot product between two points (vectors). | |
double | cloud_geometry::dot (const geometry_msgs::Vector3 &p1, const geometry_msgs::Point32 &p2) |
Compute the dot product between two points (vectors). | |
double | cloud_geometry::dot (const geometry_msgs::Point32 &p1, const geometry_msgs::Vector3 &p2) |
Compute the dot product between two points (vectors). | |
double | cloud_geometry::dot (const geometry_msgs::Point &p1, const geometry_msgs::Point32 &p2) |
Compute the dot product between two points (vectors). | |
double | cloud_geometry::dot (const geometry_msgs::Point32 &p1, const geometry_msgs::Point &p2) |
Compute the dot product between two points (vectors). | |
double | cloud_geometry::dot (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2) |
Compute the dot product between two points (vectors). | |
double | cloud_geometry::dot (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2) |
Compute the dot product between two points (vectors). | |
void | cloud_geometry::downsamplePointCloud (const sensor_msgs::PointCloud &points, sensor_msgs::PointCloud &points_down, geometry_msgs::Point leaf_size) |
Downsample a Point Cloud using a voxelized grid approach. | |
void | cloud_geometry::downsamplePointCloud (sensor_msgs::PointCloudConstPtr points, sensor_msgs::PointCloud &points_down, geometry_msgs::Point leaf_size, std::vector< Leaf > &leaves, int d_idx, double cut_distance) |
Downsample a Point Cloud using a voxelized grid approach. | |
void | cloud_geometry::downsamplePointCloud (const sensor_msgs::PointCloud &points, sensor_msgs::PointCloud &points_down, geometry_msgs::Point leaf_size, std::vector< Leaf > &leaves, int d_idx, double cut_distance) |
Downsample a Point Cloud using a voxelized grid approach. | |
void | cloud_geometry::downsamplePointCloud (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, sensor_msgs::PointCloud &points_down, geometry_msgs::Point leaf_size, std::vector< Leaf > &leaves, int d_idx, double cut_distance) |
Downsample a Point Cloud using a voxelized grid approach. | |
std::string | cloud_geometry::getAvailableChannels (const sensor_msgs::PointCloudConstPtr &cloud) |
Get the available dimensions as a space separated string. | |
std::string | cloud_geometry::getAvailableChannels (const sensor_msgs::PointCloud &cloud) |
Get the available dimensions as a space separated string. | |
int | cloud_geometry::getChannelIndex (sensor_msgs::PointCloudConstPtr points, std::string channel_name) |
Get the index of a specified dimension/channel in a point cloud. | |
int | cloud_geometry::getChannelIndex (const sensor_msgs::PointCloud &points, std::string channel_name) |
Get the index of a specified dimension/channel in a point cloud. | |
void | cloud_geometry::getCoordinateSystemOnPlane (const std::vector< double > &plane_coeff, Eigen::Vector3d &u, Eigen::Vector3d &v) |
Get a u-v-n coordinate system that lies on a plane defined by its normal. | |
void | cloud_geometry::getPointAsFloatArray (const sensor_msgs::PointCloud &points, int index, std::vector< float > &array, std::vector< int > channels) |
Create a quick copy of a point and its associated channels and return the data as a float vector. | |
void | cloud_geometry::getPointAsFloatArray (const sensor_msgs::PointCloud &points, int index, std::vector< float > &array, int start_channel, int end_channel) |
Create a quick copy of a point and its associated channels and return the data as a float vector. | |
void | cloud_geometry::getPointAsFloatArray (const sensor_msgs::PointCloud &points, int index, std::vector< float > &array, int nr_channels) |
Create a quick copy of a point and its associated channels and return the data as a float vector. | |
void | cloud_geometry::getPointAsFloatArray (const sensor_msgs::PointCloud &points, int index, std::vector< float > &array) |
Create a quick copy of a point and its associated channels and return the data as a float vector. | |
void | cloud_geometry::getPointCloud (const sensor_msgs::PointCloud &input, const std::vector< int > &indices, sensor_msgs::PointCloud &output) |
Create a new point cloud object by copying the data from a given input point cloud using a set of indices. | |
void | cloud_geometry::getPointCloudOutside (const sensor_msgs::PointCloud &input, std::vector< int > indices, sensor_msgs::PointCloud &output) |
Create a new point cloud object by copying the data from a given input point cloud using a set of indices. | |
void | cloud_geometry::getPointIndicesAxisParallelNormals (const sensor_msgs::PointCloud &points, int nx, int ny, int nz, double eps_angle, const geometry_msgs::Point32 &axis, std::vector< int > &indices) |
Get the point indices from a cloud, whose normals are close to parallel with a given axis direction. | |
void | cloud_geometry::getPointIndicesAxisPerpendicularNormals (const sensor_msgs::PointCloud &points, int nx, int ny, int nz, double eps_angle, const geometry_msgs::Point32 &axis, std::vector< int > &indices) |
Get the point indices from a cloud, whose normals are close to perpendicular to a given axis direction. | |
void | cloud_geometry::normalizePoint (geometry_msgs::Point32 &p) |
Normalize a point and return the result in place. | |
void | cloud_geometry::normalizePoint (const geometry_msgs::Point32 &p, geometry_msgs::Point32 &q) |
Normalize a point. |