#include <target_adaptive_tracking.h>
Classes | |
struct | AdjacentInfo |
struct | ReferenceModel |
Public Member Functions | |
void | backgroundReferenceLikelihood (const ModelsPtr, const ModelsPtr, std::map< uint32_t, float >) |
virtual void | callback (const sensor_msgs::PointCloud2::ConstPtr &, const geometry_msgs::PoseStamped::ConstPtr &) |
Eigen::Vector4f | cloudMeanNormal (const pcl::PointCloud< pcl::Normal >::Ptr, bool=true) |
virtual std::vector< pcl::PointIndices::Ptr > | clusterPointIndicesToPointIndices (const jsk_recognition_msgs::ClusterPointIndicesConstPtr &) |
void | compute3DCentroids (const pcl::PointCloud< PointT >::Ptr, Eigen::Vector4f &) const |
void | computeCloudClusterRPYHistogram (const pcl::PointCloud< PointT >::Ptr, const pcl::PointCloud< pcl::Normal >::Ptr, cv::Mat &) const |
float | computeCoherency (const float, const float) |
void | computeColorHistogram (const pcl::PointCloud< PointT >::Ptr, cv::Mat &, const int=16, const int=16, bool=true) const |
void | computeLocalPairwiseFeautures (const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, const std::map< uint32_t, std::vector< uint32_t > > &, cv::Mat &, const int=3) |
void | computePointFPFH (const pcl::PointCloud< PointT >::Ptr, pcl::PointCloud< pcl::Normal >::Ptr, cv::Mat &, bool=true) const |
void | computeScatterMatrix (const pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f) |
pcl::PointXYZRGBNormal | convertVector4fToPointXyzRgbNormal (const Eigen::Vector4f &, const Eigen::Vector4f &, const cv::Scalar) |
void | estimatedPFPose (const geometry_msgs::PoseStamped::ConstPtr &, PointXYZRPY &) |
template<class T > | |
void | estimatePointCloudNormals (const pcl::PointCloud< PointT >::Ptr, pcl::PointCloud< pcl::Normal >::Ptr, T=0.05f, bool=false) const |
void | filterCloudForBoundingBoxViz (pcl::PointCloud< PointT >::Ptr, const ModelsPtr, const float=0.6f) |
bool | filterPointCloud (pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f, const ModelsPtr, const float) |
template<typename T > | |
void | getRotationMatrixFromRPY (const PointXYZRPY &, Eigen::Matrix< T, 3, 3 > &) |
template<class T > | |
T | localVoxelConvexityLikelihood (Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f) |
virtual void | objInitCallback (const sensor_msgs::PointCloud2::ConstPtr &, const sensor_msgs::PointCloud2::ConstPtr &, const geometry_msgs::PoseStamped::ConstPtr &) |
template<typename T , typename U , typename V > | |
cv::Scalar | plotJetColour (T, U, V) |
void | processInitCloud (const pcl::PointCloud< PointT >::Ptr, ModelsPtr) |
void | processVoxelForReferenceModel (const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >, const std::multimap< uint32_t, uint32_t >, const uint32_t, TargetAdaptiveTracking::ReferenceModel *) |
void | publishSupervoxel (const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >, sensor_msgs::PointCloud2 &, jsk_recognition_msgs::ClusterPointIndices &, const std_msgs::Header &) |
void | supervoxelSegmentation (const pcl::PointCloud< PointT >::Ptr, std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, std::multimap< uint32_t, uint32_t > &) |
void | supervoxelSegmentation (const pcl::PointCloud< PointT >::Ptr, std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, std::multimap< uint32_t, uint32_t > &, const float) |
TargetAdaptiveTracking () | |
template<class T > | |
T | targetCandidateToReferenceLikelihood (const ReferenceModel &, const pcl::PointCloud< PointT >::Ptr, const pcl::PointCloud< pcl::Normal >::Ptr, const Eigen::Vector4f &, ReferenceModel *=NULL) |
void | targetDescriptiveSurfelsEstimationAndUpdate (pcl::PointCloud< PointT >::Ptr, const Eigen::Affine3f &, const TargetAdaptiveTracking::PointXYZRPY &, const std_msgs::Header) |
void | targetDescriptiveSurfelsIndices (const jsk_recognition_msgs::ClusterPointIndices &, const std::vector< uint32_t > &, jsk_recognition_msgs::ClusterPointIndices &) |
float | templateCloudFilterLenght (const pcl::PointCloud< PointT >::Ptr) |
void | transformModelPrimitives (const ModelsPtr &, ModelsPtr, const Eigen::Affine3f &) |
void | voxelizeAndProcessPointCloud (const pcl::PointCloud< PointT >::Ptr cloud, const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, const std::multimap< uint32_t, uint32_t > &, std::vector< AdjacentInfo > &, ModelsPtr &, bool=true, bool=true, bool=true, bool=false) |
Public Member Functions inherited from jsk_topic_tools::DiagnosticNodelet | |
DiagnosticNodelet (const std::string &name) | |
Public Member Functions inherited from jsk_topic_tools::ConnectionBasedNodelet | |
ConnectionBasedNodelet () | |
Public Member Functions inherited from nodelet::Nodelet | |
void | init (const std::string &name, const M_string &remapping_args, const V_string &my_argv, ros::CallbackQueueInterface *st_queue=NULL, ros::CallbackQueueInterface *mt_queue=NULL) |
Nodelet () | |
virtual | ~Nodelet () |
Private Types | |
typedef jsk_pcl_ros::TargetAdaptiveTrackingConfig | Config |
typedef std::vector< ReferenceModel > | Models |
typedef boost::shared_ptr< Models > | ModelsPtr |
typedef std::vector< PointXYZRPY > | MotionHistory |
typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, geometry_msgs::PoseStamped > | ObjectSyncPolicy |
typedef pcl::PointXYZRGB | PointT |
typedef pcl::tracking::ParticleXYZRPY | PointXYZRPY |
typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, geometry_msgs::PoseStamped > | SyncPolicy |
Private Member Functions | |
virtual void | configCallback (Config &, uint32_t) |
Additional Inherited Members | |
Public Types inherited from jsk_topic_tools::DiagnosticNodelet | |
typedef boost::shared_ptr< DiagnosticNodelet > | Ptr |
Protected Attributes inherited from jsk_topic_tools::DiagnosticNodelet | |
TimeredDiagnosticUpdater::Ptr | diagnostic_updater_ |
const std::string | name_ |
jsk_topic_tools::VitalChecker::Ptr | vital_checker_ |
Protected Attributes inherited from jsk_topic_tools::ConnectionBasedNodelet | |
bool | always_subscribe_ |
std::vector< image_transport::CameraPublisher > | camera_publishers_ |
boost::mutex | connection_mutex_ |
ConnectionStatus | connection_status_ |
bool | ever_subscribed_ |
std::vector< image_transport::Publisher > | image_publishers_ |
boost::shared_ptr< ros::NodeHandle > | nh_ |
bool | on_init_post_process_called_ |
boost::shared_ptr< ros::NodeHandle > | pnh_ |
std::vector< ros::Publisher > | publishers_ |
bool | subscribed_ |
ros::WallTimer | timer_warn_never_subscribed_ |
ros::WallTimer | timer_warn_on_init_post_process_called_ |
bool | verbose_connection_ |
Definition at line 66 of file target_adaptive_tracking.h.
|
private |
Definition at line 163 of file target_adaptive_tracking.h.
|
private |
Definition at line 94 of file target_adaptive_tracking.h.
|
private |
Definition at line 95 of file target_adaptive_tracking.h.
|
private |
Definition at line 97 of file target_adaptive_tracking.h.
|
private |
Definition at line 107 of file target_adaptive_tracking.h.
|
private |
Definition at line 68 of file target_adaptive_tracking.h.
|
private |
Definition at line 96 of file target_adaptive_tracking.h.
|
private |
Definition at line 100 of file target_adaptive_tracking.h.
jsk_pcl_ros::TargetAdaptiveTracking::TargetAdaptiveTracking | ( | ) |
Definition at line 10 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::backgroundReferenceLikelihood | ( | const ModelsPtr | background_reference, |
const ModelsPtr | target_voxels, | ||
std::map< uint32_t, float > | max_prob | ||
) |
Definition at line 1139 of file target_adaptive_tracking_nodelet.cpp.
|
virtual |
Definition at line 162 of file target_adaptive_tracking_nodelet.cpp.
Eigen::Vector4f jsk_pcl_ros::TargetAdaptiveTracking::cloudMeanNormal | ( | const pcl::PointCloud< pcl::Normal >::Ptr | normal, |
bool | isnorm = true |
||
) |
Definition at line 1400 of file target_adaptive_tracking_nodelet.cpp.
|
virtual |
Definition at line 1329 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::compute3DCentroids | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
Eigen::Vector4f & | centre | ||
) | const |
Definition at line 1384 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::computeCloudClusterRPYHistogram | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
const pcl::PointCloud< pcl::Normal >::Ptr | normal, | ||
cv::Mat & | histogram | ||
) | const |
Definition at line 1186 of file target_adaptive_tracking_nodelet.cpp.
float jsk_pcl_ros::TargetAdaptiveTracking::computeCoherency | ( | const float | dist, |
const float | weight | ||
) |
Definition at line 1459 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::computeColorHistogram | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
cv::Mat & | hist, | ||
const int | hBin = 16 , |
||
const int | sBin = 16 , |
||
bool | is_norm = true |
||
) | const |
Definition at line 1263 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::computeLocalPairwiseFeautures | ( | const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > & | supervoxel_clusters, |
const std::map< uint32_t, std::vector< uint32_t > > & | adjacency_list, | ||
cv::Mat & | histogram, | ||
const int | feature_count = 3 |
||
) |
Definition at line 1501 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::computePointFPFH | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
pcl::PointCloud< pcl::Normal >::Ptr | normals, | ||
cv::Mat & | histogram, | ||
bool | holistic = true |
||
) | const |
Definition at line 1290 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::computeScatterMatrix | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
const Eigen::Vector4f | centroid | ||
) |
Definition at line 1432 of file target_adaptive_tracking_nodelet.cpp.
|
privatevirtual |
Definition at line 1849 of file target_adaptive_tracking_nodelet.cpp.
pcl::PointXYZRGBNormal jsk_pcl_ros::TargetAdaptiveTracking::convertVector4fToPointXyzRgbNormal | ( | const Eigen::Vector4f & | centroid, |
const Eigen::Vector4f & | normal, | ||
const cv::Scalar | color | ||
) |
Definition at line 1468 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::estimatedPFPose | ( | const geometry_msgs::PoseStamped::ConstPtr & | pose_msg, |
PointXYZRPY & | motion_displacement | ||
) |
Definition at line 1342 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::estimatePointCloudNormals | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
pcl::PointCloud< pcl::Normal >::Ptr | normals, | ||
T | k = 0.05f , |
||
bool | use_knn = false |
||
) | const |
Definition at line 1166 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::filterCloudForBoundingBoxViz | ( | pcl::PointCloud< PointT >::Ptr | cloud, |
const ModelsPtr | background_reference, | ||
const float | threshold = 0.6f |
||
) |
Definition at line 1687 of file target_adaptive_tracking_nodelet.cpp.
bool jsk_pcl_ros::TargetAdaptiveTracking::filterPointCloud | ( | pcl::PointCloud< PointT >::Ptr | cloud, |
const Eigen::Vector4f | tracker_position, | ||
const ModelsPtr | template_model, | ||
const float | scaling_factor | ||
) |
Definition at line 1616 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::getRotationMatrixFromRPY | ( | const PointXYZRPY & | motion_displacement, |
Eigen::Matrix< T, 3, 3 > & | rotation | ||
) |
Definition at line 1486 of file target_adaptive_tracking_nodelet.cpp.
T jsk_pcl_ros::TargetAdaptiveTracking::localVoxelConvexityLikelihood | ( | Eigen::Vector4f | c_centroid, |
Eigen::Vector4f | c_normal, | ||
Eigen::Vector4f | n_centroid, | ||
Eigen::Vector4f | n_normal | ||
) |
Definition at line 1115 of file target_adaptive_tracking_nodelet.cpp.
|
virtual |
Definition at line 102 of file target_adaptive_tracking_nodelet.cpp.
|
protectedvirtual |
Reimplemented from jsk_topic_tools::DiagnosticNodelet.
Definition at line 22 of file target_adaptive_tracking_nodelet.cpp.
cv::Scalar jsk_pcl_ros::TargetAdaptiveTracking::plotJetColour | ( | T | v, |
U | vmin, | ||
V | vmax | ||
) |
Definition at line 1734 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::processInitCloud | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
ModelsPtr | object_reference | ||
) |
Definition at line 135 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::processVoxelForReferenceModel | ( | const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > | supervoxel_clusters, |
const std::multimap< uint32_t, uint32_t > | supervoxel_adjacency, | ||
const uint32_t | match_index, | ||
TargetAdaptiveTracking::ReferenceModel * | ref_model | ||
) |
Definition at line 1022 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::publishSupervoxel | ( | const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > | supervoxel_clusters, |
sensor_msgs::PointCloud2 & | ros_cloud, | ||
jsk_recognition_msgs::ClusterPointIndices & | ros_indices, | ||
const std_msgs::Header & | header | ||
) |
Definition at line 1806 of file target_adaptive_tracking_nodelet.cpp.
|
protectedvirtual |
Implements jsk_topic_tools::ConnectionBasedNodelet.
Definition at line 72 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::supervoxelSegmentation | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > & | supervoxel_clusters, | ||
std::multimap< uint32_t, uint32_t > & | supervoxel_adjacency | ||
) |
Definition at line 1784 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::supervoxelSegmentation | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > & | supervoxel_clusters, | ||
std::multimap< uint32_t, uint32_t > & | supervoxel_adjacency, | ||
const float | seed_resolution | ||
) |
Definition at line 1760 of file target_adaptive_tracking_nodelet.cpp.
T jsk_pcl_ros::TargetAdaptiveTracking::targetCandidateToReferenceLikelihood | ( | const ReferenceModel & | reference_model, |
const pcl::PointCloud< PointT >::Ptr | cloud, | ||
const pcl::PointCloud< pcl::Normal >::Ptr | normal, | ||
const Eigen::Vector4f & | centroid, | ||
ReferenceModel * | voxel_model = NULL |
||
) |
Definition at line 1074 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::targetDescriptiveSurfelsEstimationAndUpdate | ( | pcl::PointCloud< PointT >::Ptr | cloud, |
const Eigen::Affine3f & | transformation_matrix, | ||
const TargetAdaptiveTracking::PointXYZRPY & | motion_disp, | ||
const std_msgs::Header | header | ||
) |
Definition at line 351 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::targetDescriptiveSurfelsIndices | ( | const jsk_recognition_msgs::ClusterPointIndices & | sv_indices, |
const std::vector< uint32_t > & | tdp_list, | ||
jsk_recognition_msgs::ClusterPointIndices & | ros_indices | ||
) |
Definition at line 1836 of file target_adaptive_tracking_nodelet.cpp.
float jsk_pcl_ros::TargetAdaptiveTracking::templateCloudFilterLenght | ( | const pcl::PointCloud< PointT >::Ptr | cloud | ) |
Definition at line 1599 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::transformModelPrimitives | ( | const ModelsPtr & | obj_ref, |
ModelsPtr | trans_models, | ||
const Eigen::Affine3f & | transform_model | ||
) |
Definition at line 1664 of file target_adaptive_tracking_nodelet.cpp.
|
protectedvirtual |
Implements jsk_topic_tools::ConnectionBasedNodelet.
Definition at line 94 of file target_adaptive_tracking_nodelet.cpp.
void jsk_pcl_ros::TargetAdaptiveTracking::voxelizeAndProcessPointCloud | ( | const pcl::PointCloud< PointT >::Ptr | cloud, |
const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > & | supervoxel_clusters, | ||
const std::multimap< uint32_t, uint32_t > & | supervoxel_adjacency, | ||
std::vector< AdjacentInfo > & | supervoxel_list, | ||
TargetAdaptiveTracking::ModelsPtr & | models, | ||
bool | norm_flag = true , |
||
bool | feat_flag = true , |
||
bool | cent_flag = true , |
||
bool | neigh_pfh = false |
||
) |
Definition at line 268 of file target_adaptive_tracking_nodelet.cpp.
|
private |
Definition at line 128 of file target_adaptive_tracking.h.
|
private |
Definition at line 152 of file target_adaptive_tracking.h.
|
private |
Definition at line 141 of file target_adaptive_tracking.h.
|
private |
Definition at line 143 of file target_adaptive_tracking.h.
|
private |
Definition at line 156 of file target_adaptive_tracking.h.
|
private |
Definition at line 131 of file target_adaptive_tracking.h.
|
private |
Definition at line 153 of file target_adaptive_tracking.h.
|
private |
Definition at line 154 of file target_adaptive_tracking.h.
|
private |
Definition at line 136 of file target_adaptive_tracking.h.
|
private |
Definition at line 160 of file target_adaptive_tracking.h.
|
private |
Definition at line 126 of file target_adaptive_tracking.h.
|
private |
Definition at line 150 of file target_adaptive_tracking.h.
|
private |
Definition at line 129 of file target_adaptive_tracking.h.
|
private |
Definition at line 161 of file target_adaptive_tracking.h.
|
private |
Definition at line 145 of file target_adaptive_tracking.h.
|
private |
Definition at line 112 of file target_adaptive_tracking.h.
|
private |
Definition at line 127 of file target_adaptive_tracking.h.
|
private |
Definition at line 140 of file target_adaptive_tracking.h.
|
private |
Definition at line 137 of file target_adaptive_tracking.h.
|
private |
Definition at line 132 of file target_adaptive_tracking.h.
|
private |
Definition at line 135 of file target_adaptive_tracking.h.
|
private |
Definition at line 134 of file target_adaptive_tracking.h.
|
private |
Definition at line 122 of file target_adaptive_tracking.h.
|
private |
Definition at line 115 of file target_adaptive_tracking.h.
|
private |
Definition at line 121 of file target_adaptive_tracking.h.
|
private |
Definition at line 119 of file target_adaptive_tracking.h.
|
private |
Definition at line 123 of file target_adaptive_tracking.h.
|
private |
Definition at line 124 of file target_adaptive_tracking.h.
|
private |
Definition at line 118 of file target_adaptive_tracking.h.
|
private |
Definition at line 117 of file target_adaptive_tracking.h.
|
private |
Definition at line 120 of file target_adaptive_tracking.h.
|
private |
Definition at line 116 of file target_adaptive_tracking.h.
|
private |
Definition at line 147 of file target_adaptive_tracking.h.
|
private |
Definition at line 144 of file target_adaptive_tracking.h.
|
private |
Definition at line 165 of file target_adaptive_tracking.h.
|
private |
Definition at line 157 of file target_adaptive_tracking.h.
|
private |
Definition at line 109 of file target_adaptive_tracking.h.
|
private |
Definition at line 101 of file target_adaptive_tracking.h.
|
private |
Definition at line 108 of file target_adaptive_tracking.h.
|
private |
Definition at line 110 of file target_adaptive_tracking.h.
|
private |
Definition at line 102 of file target_adaptive_tracking.h.
|
private |
Definition at line 103 of file target_adaptive_tracking.h.
|
private |
Definition at line 151 of file target_adaptive_tracking.h.
|
private |
Definition at line 133 of file target_adaptive_tracking.h.
|
private |
Definition at line 130 of file target_adaptive_tracking.h.
|
private |
Definition at line 159 of file target_adaptive_tracking.h.
|
private |
Definition at line 158 of file target_adaptive_tracking.h.
|
private |
Definition at line 139 of file target_adaptive_tracking.h.
|
private |
Definition at line 148 of file target_adaptive_tracking.h.
|
private |
Definition at line 155 of file target_adaptive_tracking.h.
|
private |
Definition at line 146 of file target_adaptive_tracking.h.