#include <ProbabilisticSecondarySceneObjectVisualization.h>
Public Member Functions | |
void | appendKernel (const boost::shared_ptr< Eigen::Vector3d > &pMean, const boost::shared_ptr< Eigen::Matrix3d > &pCovariance) |
void | overwriteLastBestPose () |
ProbabilisticSecondarySceneObjectVisualization () | |
ProbabilisticSecondarySceneObjectVisualization (std::string pName) | |
void | resetCertainty () |
void | setAbsoluteLearningSamples (const std::vector< Eigen::Vector3d > &pRelativeSamples, const std::vector< Eigen::Vector3d > &pAbsoluteSamples, const std::vector< Eigen::Vector3d > &pAbsoluteParentSamples) |
void | setBestCandidatePose (boost::shared_ptr< ISM::Pose > pPose) |
void | setBestPoseCandidate (double pScore) |
void | setColors (const std_msgs::ColorRGBA &pFirstRingColor, const std_msgs::ColorRGBA &pSecondRingColor) |
void | setDetectionCertainty (double pCertainty) |
void | setDrawingParameters (const double pScale, const float pSigmaMultiplicator, const std::string pFrameId) |
void | setLastPose (boost::shared_ptr< ISM::Pose > pPose) |
void | setNamespace (std::string pNamespace) |
void | setParentPose (boost::shared_ptr< ISM::Pose > pPose) |
void | visualizeAbsoluteTrajectory (unsigned int &pMarkerId) |
void | visualizeGaussianKernels (unsigned int &pMarkerId) |
void | visualizeKinematicChain (unsigned int &pMarkerId) |
void | visualizeObjectPositionAsArrow (unsigned int &pMarkerId) |
void | visualizeRelativeTrajectory (unsigned int &pMarkerId) |
~ProbabilisticSecondarySceneObjectVisualization () | |
Private Attributes | |
std::vector< Eigen::Vector3d > | mAbsoluteParentSamples |
std::vector< Eigen::Vector3d > | mAbsoluteSamples |
boost::shared_ptr< ISM::Pose > | mBestCandidatePose |
boost::shared_ptr< ISM::Pose > | mBestPose |
double | mBestScore |
double | mBestScoreCandidate |
double | mCertainty |
ros::NodeHandle | mHandle |
std::vector< std::pair< Eigen::Vector3d, Eigen::Matrix3d > > | mKernels |
boost::shared_ptr< ISM::Pose > | mLastObjectPose |
boost::shared_ptr< ISM::Pose > | mParentPose |
boost::shared_ptr< ros::Publisher > | mPublisher |
std::vector< Eigen::Vector3d > | mRelativeSamples |
std::string | mSceneObject |
CoordinateFrameVisualizer | visualizerFrame |
GaussianKernelVisualizer | visualizerKernel |
KinematicChainVisualizer | visualizerKinematics |
SampleVisualizer | visualizerSamples |
Visualizer class for secondary scenes objects.
Definition at line 53 of file ProbabilisticSecondarySceneObjectVisualization.h.
Visualization::ProbabilisticSecondarySceneObjectVisualization::ProbabilisticSecondarySceneObjectVisualization | ( | ) |
Constructor.
Definition at line 22 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
Visualization::ProbabilisticSecondarySceneObjectVisualization::ProbabilisticSecondarySceneObjectVisualization | ( | std::string | pName | ) |
Constructor.
pName | The name of this secondary scene object. |
Definition at line 30 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
Visualization::ProbabilisticSecondarySceneObjectVisualization::~ProbabilisticSecondarySceneObjectVisualization | ( | ) |
Destructor.
Definition at line 39 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::appendKernel | ( | const boost::shared_ptr< Eigen::Vector3d > & | pMean, |
const boost::shared_ptr< Eigen::Matrix3d > & | pCovariance | ||
) |
Appends a kernel that will be visualized in form of a covariance ellipse.
pCovariance | A 3x3 eigen matrix representing the corvariance matrix of the gaussian kernel. |
pMean | A 3d eigen vector representing the mean of the gaussian kernel. |
Definition at line 151 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::overwriteLastBestPose | ( | ) |
Takes the last stored candidate for the best pose as the last best pose. This method is invoked by the primary scene object.
Definition at line 145 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::resetCertainty | ( | ) |
Resets the certainty value that describes the fitting of the associated scene object.
Definition at line 97 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setAbsoluteLearningSamples | ( | const std::vector< Eigen::Vector3d > & | pRelativeSamples, |
const std::vector< Eigen::Vector3d > & | pAbsoluteSamples, | ||
const std::vector< Eigen::Vector3d > & | pAbsoluteParentSamples | ||
) |
Sets the samples used for model learning and the corresponding poses for the parent object.
pRelativeSamples | The list of relative poses to the parent. |
pAbsoluteSamples | The list of absolute samples used for model learning. |
pAbsoluteParentSamples | The list of corresponding poses for the parent object. |
Definition at line 136 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setBestCandidatePose | ( | boost::shared_ptr< ISM::Pose > | pPose | ) |
Sets a candidate for the absolute pose of the associated object. The primary scene object will decide based on the hypothsis score, if it is the best absolute pose.
pPose | Best absolute pose of the primary scene object. |
Definition at line 111 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setBestPoseCandidate | ( | double | pScore | ) |
Sets a candidate pScore for the link between this object and its parent. The primary scene object will decide based on the hypothsis score, if it is the best score for the link.
pScore | The score. |
Definition at line 121 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setColors | ( | const std_msgs::ColorRGBA & | pFirstRingColor, |
const std_msgs::ColorRGBA & | pSecondRingColor | ||
) |
Sets the colors of the two rings around the covariance ellipse.
pFirstRingColor | The color of the first ring. |
pSecondRingColor | The color of the second ring. |
Definition at line 72 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setDetectionCertainty | ( | double | pCertainty | ) |
Sets the certainty value that describes the fitting of the associated scene object.
pCertainty | The certainty betwween 0 and 1. |
Definition at line 86 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setDrawingParameters | ( | const double | pScale, |
const float | pSigmaMultiplicator, | ||
const std::string | pFrameId | ||
) |
Sets the parameter requried for visualizing the secondary scene object.
pScale | Factor to multiply the kernel with. |
pSigmaMultiplicator | Scaling factor for the size of the visualized covariance ellipsoid. |
pFrameId | The name of the coordinate frame that should be drawn into. |
Definition at line 43 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setLastPose | ( | boost::shared_ptr< ISM::Pose > | pPose | ) |
Sets the last known pose of the associated object.
pPose | The last known pose. |
Definition at line 102 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setNamespace | ( | std::string | pNamespace | ) |
Sets the name space bases on the names of scene and scene object.
pNamespace | The namespace to use for visualization. |
Definition at line 58 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::setParentPose | ( | boost::shared_ptr< ISM::Pose > | pPose | ) |
Sets the absolute pose of the parent object.
pPose | Absolute pose of the primary scene object. |
Definition at line 126 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::visualizeAbsoluteTrajectory | ( | unsigned int & | pMarkerId | ) |
Visualizes the absolute trajectory.
pMarkerId | An unique id for the marker. Any marker sent with the same id will overwrite the old one. |
Definition at line 231 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::visualizeGaussianKernels | ( | unsigned int & | pMarkerId | ) |
Visualizes all gaussian kernels given to this class.
pMarkerId | An unique id for the marker. Any marker sent with the same id will overwrite the old one. |
Definition at line 166 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::visualizeKinematicChain | ( | unsigned int & | pMarkerId | ) |
Visualizes the kinematic chain.
pMarkerId | An unique id for the marker. Any marker sent with the same id will overwrite the old one. |
Definition at line 199 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::visualizeObjectPositionAsArrow | ( | unsigned int & | pMarkerId | ) |
Visualizes the position of this secondary scene object.
pMarkerId | Serves as unique id for the marker. Any marker sent with the same id will overwrite the old one. |
Definition at line 184 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
void Visualization::ProbabilisticSecondarySceneObjectVisualization::visualizeRelativeTrajectory | ( | unsigned int & | pMarkerId | ) |
Visualizes the trajectory composed of the samples used for learning in relation to the parent object.
pMarkerId | An unique id for the marker. Any marker sent with the same id will overwrite the old one. |
Definition at line 214 of file ProbabilisticSecondarySceneObjectVisualization.cpp.
|
private |
Definition at line 278 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Definition at line 278 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Candidate for the best absolute position of the associated object. It was determined by the best scoring hypothesis regarding the assignment of evidence to the slots.
Definition at line 263 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Best absolute pose of the associated object. It was determined by the best scoring hypothesis regarding the assignment of evidence to the slots.
Definition at line 258 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
The best score for the link to the parent.
Definition at line 243 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Candidate for the best score for the link to the parent.
Definition at line 248 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
The uncertainty describes how good the associated scene object fits into the gaussian distribution.
Definition at line 238 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Node handle for inintializing the publisher.
Definition at line 208 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
The kernels that will be visualized in form of covariance ellipses.
Definition at line 273 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
The last known pose of the object associated with this visualizer.
Definition at line 253 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Absolute pose of the parent object.
Definition at line 268 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Publisher for all visualization messages generated in this class.
Definition at line 213 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
The samples used for model learning and the corresponding poses for the parent object.
Definition at line 278 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
The name of this secondary scene object.
Definition at line 203 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Visualization helper for coordinate frames.
Definition at line 218 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Visualization helper for gaussian kernels.
Definition at line 223 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Visualization helper for the kinematic chain.
Definition at line 228 of file ProbabilisticSecondarySceneObjectVisualization.h.
|
private |
Visualization helper for point clound.
Definition at line 233 of file ProbabilisticSecondarySceneObjectVisualization.h.