#include <GaussianKernelVisualizer.h>

Inheritance diagram for Visualization::GaussianKernelVisualizer:

Public Member Functions
	GaussianKernelVisualizer ()

void	publishKernel (boost::shared_ptr< ros::Publisher > pPublisher, unsigned int &pMarkerId, const Eigen::Vector3d &pMean, const Eigen::Matrix3d &pCovariance)

void	setParentPose (boost::shared_ptr< ISM::Pose > pPose)

void	setSigmaMultiplicator (const float pSigmaMultiplicator)

	~GaussianKernelVisualizer ()

Public Member Functions inherited from Visualization::AbstractExtendedVisualizer
	AbstractExtendedVisualizer ()

void	setColors (const std_msgs::ColorRGBA &pFirstColor, const std_msgs::ColorRGBA &pSecondColor, const std_msgs::ColorRGBA &pThirdColor)

void	setScaleFactor (const double pScaleFactor)

virtual	~AbstractExtendedVisualizer ()

Public Member Functions inherited from Visualization::AbstractVisualizer
	AbstractVisualizer ()

void	setFrameId (std::string pFrameId)

void	setNamespace (std::string pNamespace)

virtual	~AbstractVisualizer ()

Private Member Functions
void	calculateCoordinatePlaneEllipses (const std::vector< int > &pEllipsoidHalfAxes, const int pDiscretizationResolution, std::vector< std::vector< std::vector< int > > > &pEllipsePolygons)

visualization_msgs::Marker	generateArrowMessage (unsigned int &pMarkerId, Eigen::Vector3d pFrom, Eigen::Vector3d pTo)

visualization_msgs::Marker	generateAxis (const unsigned int pMarkerId, const unsigned int pAxis, const Eigen::Vector3d &pMean, const Eigen::Matrix3d &pCovariance)

visualization_msgs::Marker	generateCovarianceEllipsoidMarker (const unsigned int pMarkerId, const int pMarkerShape, const std::vector< std::vector< float > > &pWireFramePolygons, const std_msgs::ColorRGBA &pColor, const Eigen::Vector3d &pMean, const Eigen::Matrix3d &pCovariance)

visualization_msgs::MarkerArray	generateSamplesMarker (const unsigned int pMarkerId, const std::vector< boost::shared_ptr< Eigen::Quaternion< double > > > &pSamples)

visualization_msgs::Marker	generateSphereMessage (unsigned int &pMarkerId, Eigen::Vector3d pPos)

float	getSigmaMultiplicator ()

template<class T >
void	stlVectorToPointMsg (const std::vector< T > pPositionVector, geometry_msgs::Point &pROSPositionMsg)

Private Attributes
boost::shared_ptr< ISM::Pose >	mParentPose

float	mSigmaMultiplicator

Additional Inherited Members
Protected Member Functions inherited from Visualization::AbstractExtendedVisualizer
const std_msgs::ColorRGBA	getColor (unsigned int pNumber)

double	getScaleFactor ()

Protected Member Functions inherited from Visualization::AbstractVisualizer
std::string	getFrameId ()

std::string	getNamespace ()

Detailed Description

Visualizer class for gaussian kernels.

Author: Joachim Gehrung

Version: See SVN

Definition at line 55 of file GaussianKernelVisualizer.h.

Constructor & Destructor Documentation

Visualization::GaussianKernelVisualizer::GaussianKernelVisualizer ( )

Constructor.

Definition at line 22 of file GaussianKernelVisualizer.cpp.

Visualization::GaussianKernelVisualizer::~GaussianKernelVisualizer ( )

Destructor.

Definition at line 27 of file GaussianKernelVisualizer.cpp.

Member Function Documentation

void Visualization::GaussianKernelVisualizer::calculateCoordinatePlaneEllipses	(	const std::vector< int > &	pEllipsoidHalfAxes,
		const int	pDiscretizationResolution,
		std::vector< std::vector< std::vector< int > > > &	pEllipsePolygons
	)

private

Calculate cross sections of coordinate planes passing through ellipsoid center and having one of the axes of the frame of the ellipsoid as normal with a three dimensional ellipsoid itself. Approximate them with polgygons and return one line strips for each of the ellipses.

Parameters

pEllipsoidHalfAxes	Discrete lengths of the ellipsoid axes in x/y/z order relative to pose of ellipsoid.
pDiscretizationResolution	Discrete angle between the subsequent polyline vertices in degrees. It defines the approximation accuracy.
pEllipsePolygons	Contour polygons (middle) containing discrete 3D points (inner) approximating ellipses in the three (outer vector) coordinate planes with normals in x/y/z order.

Definition at line 177 of file GaussianKernelVisualizer.cpp.

visualization_msgs::Marker Visualization::GaussianKernelVisualizer::generateArrowMessage	(	unsigned int &	pMarkerId,
		Eigen::Vector3d	pFrom,
		Eigen::Vector3d	pTo
	)

private

Generates an arrow between two points.

Parameters

pMarkerId	Serves as unique id for the marker. Any marker sent with the same id will overwrite the old one.
pFrom	The position to start the link from.
pTo	The position to end the link.

Definition at line 581 of file GaussianKernelVisualizer.cpp.

visualization_msgs::Marker Visualization::GaussianKernelVisualizer::generateAxis	(	const unsigned int	pMarkerId,
		const unsigned int	pAxis,
		const Eigen::Vector3d &	pMean,
		const Eigen::Matrix3d &	pCovariance
	)

private

Generates the visualization message that contains the coordinate system base vectors of the covariance ellipse.

Parameters

pMarkerId	An unique id for the marker. Any marker sent with the same id will overwrite the old one.
pAxis	The number of the axis that the message should be created for.
pMean	The mean vector of the kernel that should be visualized.
pCovariance	The covariance matrix of the kernel that should be visualized.

Returns: A marker array holding the visualization messages.

Definition at line 378 of file GaussianKernelVisualizer.cpp.

visualization_msgs::Marker Visualization::GaussianKernelVisualizer::generateCovarianceEllipsoidMarker	(	const unsigned int	pMarkerId,
		const int	pMarkerShape,
		const std::vector< std::vector< float > > &	pWireFramePolygons,
		const std_msgs::ColorRGBA &	pColor,
		const Eigen::Vector3d &	pMean,
		const Eigen::Matrix3d &	pCovariance
	)

private

Generates the visualization message for a covariance ellipse, the rings and the coordinate system.

Parameters

pMarkerId	An unique id for the marker. Any marker sent with the same id will overwrite the old one.
pMarkerShape	Should be either visualization_msgs::Marker::SPHERE for the ellipsoid or LINE_STRIP for the rings.
pWireFramePolygons	Contour polygons (outer) containing 3D points (inner) approximating ellipses in one coordinate planewith normal in x/y/z order. Assign emtpy vector if surface model should be generated.
pColor	The r/g/b/a color for the marker.
pMean	The mean vector of the kernel that should be visualized.
pCovariance	The covariance matrix of the kernel that should be visualized.

Returns: A marker array holding the visualization messages.

Definition at line 236 of file GaussianKernelVisualizer.cpp.

visualization_msgs::MarkerArray Visualization::GaussianKernelVisualizer::generateSamplesMarker	(	const unsigned int	pMarkerId,
		const std::vector< boost::shared_ptr< Eigen::Quaternion< double > > > &	pSamples
	)

private

Create a visualization marker message that contains all samples.

Parameters

pMarkerId	Serves as unique id for the marker. Any marker sent with the same id will overwrite the old one.
pSamples	The list of all samples.

Returns: A marker array holding the visualization messages.

Definition at line 478 of file GaussianKernelVisualizer.cpp.

visualization_msgs::Marker Visualization::GaussianKernelVisualizer::generateSphereMessage	(	unsigned int &	pMarkerId,
		Eigen::Vector3d	pPos
	)

private

Generates a small spherical marker at the given position.

Parameters

pMarkerId	Serves as unique id for the marker. Any marker sent with the same id will overwrite the old one.
pPos	The position of the marker.

Definition at line 538 of file GaussianKernelVisualizer.cpp.

float Visualization::GaussianKernelVisualizer::getSigmaMultiplicator ( )

private

Returns the sigma multiplicator.

Returns: Scaling factor for the size of the visualized covariance ellipsoid.

void Visualization::GaussianKernelVisualizer::publishKernel	(	boost::shared_ptr< ros::Publisher >	pPublisher,
		unsigned int &	pMarkerId,
		const Eigen::Vector3d &	pMean,
		const Eigen::Matrix3d &	pCovariance
	)

Publishes a single gaussian kernel.

Parameters

pPublisher	For publishing the visualization message.
pMarkerId	Serves as unique id for the marker. Any marker sent with the same id will overwrite the old one.
pMean	The mean vector of the kernel that should be visualized.
pCovariance	The covariance matrix of the kernel that should be visualized.

Definition at line 46 of file GaussianKernelVisualizer.cpp.

void Visualization::GaussianKernelVisualizer::setParentPose ( boost::shared_ptr< ISM::Pose > pPose )

Sets the absolute pose of the parent object.

Parameters

pPose Absolute pose of the primary scene object.

Definition at line 36 of file GaussianKernelVisualizer.cpp.

void Visualization::GaussianKernelVisualizer::setSigmaMultiplicator ( const float pSigmaMultiplicator )

Sets the sigma multiplicator.

Parameters

pSigmaMultiplicator Scaling factor for the size of the visualized covariance ellipsoid.

Definition at line 31 of file GaussianKernelVisualizer.cpp.

template<class T >

void Visualization::GaussianKernelVisualizer::stlVectorToPointMsg	(	const std::vector< T >	pPositionVector,
		geometry_msgs::Point &	pROSPositionMsg
	)

private

Converts a 3D std::vector<T> of arbitrary compatible type T into a geometry_msgs/Point.msg from ROS.

Parameters

pPositionVector	STL representation of vector consisting of concatenated cartesian coordinates.
pROSPositionMsg	ROS message containing position information as cartesian point in 3D space.