00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef JSK_PCL_ROS_PLANE_SUPPORTED_CUBOID_ESTIMATOR_H_
00038 #define JSK_PCL_ROS_PLANE_SUPPORTED_CUBOID_ESTIMATOR_H_
00039
00040 #include <jsk_topic_tools/diagnostic_nodelet.h>
00041 #include <message_filters/subscriber.h>
00042 #include <message_filters/time_synchronizer.h>
00043 #include <message_filters/synchronizer.h>
00044 #include <boost/random.hpp>
00045 #include <dynamic_reconfigure/server.h>
00046 #include "jsk_pcl_ros/geo_util.h"
00047 #include "jsk_pcl_ros/pcl_conversion_util.h"
00048 #include "jsk_pcl_ros/tf_listener_singleton.h"
00049 #include <algorithm>
00050
00051
00052
00053 #include <jsk_recognition_msgs/PolygonArray.h>
00054 #include <jsk_recognition_msgs/BoundingBoxArray.h>
00055 #include <jsk_recognition_msgs/ModelCoefficientsArray.h>
00056 #include <jsk_recognition_msgs/HistogramWithRange.h>
00057 #include <jsk_pcl_ros/PlaneSupportedCuboidEstimatorConfig.h>
00058 #include <sensor_msgs/PointCloud2.h>
00059 #include <std_srvs/Empty.h>
00060
00061
00062 #include <pcl/tracking/tracking.h>
00063 #include "jsk_pcl_ros/pcl/simple_particle_filter.h"
00064 #include "jsk_pcl_ros/pcl/particle_cuboid.h"
00065 #include <pcl/kdtree/kdtree_flann.h>
00066
00067 namespace jsk_pcl_ros
00068 {
00069
00070
00075 inline double binaryLikelihood(double v, double min, double max)
00076 {
00077 if (v < min) {
00078 return 0;
00079 }
00080 else if (v > max) {
00081 return 0;
00082 }
00083 else {
00084 return 1;
00085 }
00086 }
00087
00088 template <class Config>
00089 double rangeLikelihood(const pcl::tracking::ParticleCuboid& p,
00090 pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
00091 const std::vector<Polygon::Ptr>& planes,
00092 const Config& config)
00093 {
00094 double likelihood = 1.0;
00095 Polygon::Ptr plane = planes[p.plane_index];
00096 if (p.plane_index == -1) {
00097
00098 }
00099 else {
00100 Eigen::Vector3f projected_point;
00101 plane->project(Eigen::Vector3f(p.getVector3fMap()), projected_point);
00102 if (plane->isInside(projected_point)) {
00103 likelihood *= 1.0;
00104 }
00105 else {
00106 likelihood = 0.0;
00107 }
00108 }
00109 float local_z = plane->distanceToPoint(Eigen::Vector3f(p.getVector3fMap()));
00110 likelihood *= binaryLikelihood(local_z, config.range_likelihood_local_min_z, config.range_likelihood_local_max_z);
00111 return likelihood;
00112 }
00113
00114 template <class Config>
00115 double supportPlaneAngularLikelihood(
00116 const pcl::tracking::ParticleCuboid& p,
00117 const std::vector<Polygon::Ptr>& planes,
00118 const Config& config)
00119 {
00120 Polygon::Ptr plane = planes[p.plane_index];
00121 if (config.use_support_plane_angular_likelihood) {
00122 double cos_likelihood = (p.toEigenMatrix().rotation() * Eigen::Vector3f::UnitZ()).dot(plane->getNormal());
00123
00124 return pow(std::abs(cos_likelihood),
00125 config.support_plane_angular_likelihood_weight_power);
00126 }
00127 else {
00128 return 1.0;
00129 }
00130 }
00131
00132
00133 template <class Config>
00134 double surfaceAreaLikelihood(
00135 const pcl::tracking::ParticleCuboid& p,
00136 const Config& config)
00137 {
00138 if (config.use_surface_area_likelihood) {
00139 double v = p.area();
00140 return 1.0 / (1.0 + pow(v, config.surface_area_error_power));
00141 }
00142 else {
00143 return 1.0;
00144 }
00145 }
00146
00147 template <class Config>
00148 double distanceFromPlaneBasedError(
00149 const pcl::tracking::ParticleCuboid& p,
00150 pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
00151 pcl::KdTreeFLANN<pcl::PointXYZ>& tree,
00152 const Eigen::Vector3f& viewpoint,
00153 const Config& config)
00154 {
00155 Eigen::Affine3f pose = p.toEigenMatrix();
00156 Eigen::Affine3f pose_inv = pose.inverse();
00157 const Eigen::Vector3f local_vp = pose_inv * viewpoint;
00158 std::vector<int> visible_faces = p.visibleFaceIndices(local_vp);
00159 double r = sqrt(p.dx * p.dx + p.dy * p.dy + p.dz * p.dz) / 2.0;
00160 std::vector<int> candidate_point_indices;
00161 std::vector<float> candidate_point_distances;
00162 pcl::PointXYZ xyz_point;
00163 xyz_point.getVector3fMap() = p.getVector3fMap();
00164
00165 tree.radiusSearch(xyz_point, r + config.outlier_distance, candidate_point_indices, candidate_point_distances);
00166 if (candidate_point_indices.size() < config.min_inliers) {
00167 return 0;
00168 }
00169 else {
00170 double error = 0.0;
00171 size_t inliers = 0;
00172 Cube::Ptr cube = p.toCube();
00173 std::vector<Polygon::Ptr> faces = cube->faces();
00174 for (size_t i = 0; i < candidate_point_indices.size(); i++) {
00175 int index = candidate_point_indices[i];
00176 Eigen::Vector3f v = cloud->points[index].getVector3fMap();
00177 if (config.use_occlusion_likelihood) {
00178 double d = p.distanceNearestToPlaneWithOcclusion(v, visible_faces, faces);
00179 if (d < config.outlier_distance) {
00180
00181 error += pow(d, config.plane_distance_error_power);
00182 ++inliers;
00183 }
00184 }
00185 else {
00186 Eigen::Vector3f local_v = pose_inv * v;
00187 double d = p.distanceToPlane(local_v, p.nearestPlaneIndex(local_v));
00188 if (d < config.outlier_distance) {
00189
00190 error += pow(d, config.plane_distance_error_power);
00191 ++inliers;
00192 }
00193 }
00194 }
00195
00196 if (inliers < config.min_inliers) {
00197 return 0;
00198 }
00199 else {
00200 return 1 / (1 + error / inliers);
00201 }
00202 }
00203 }
00204
00205 template <class Config>
00206 double planeLikelihood(const pcl::tracking::ParticleCuboid& p,
00207 const std::vector<float>& polygon_likelihood,
00208 const Config& config)
00209 {
00210 if (config.use_polygon_likelihood) {
00211 return polygon_likelihood[p.plane_index];
00212 }
00213 else {
00214 return 1.0;
00215 }
00216 }
00217
00218 template <class Config>
00219 double computeLikelihood(const pcl::tracking::ParticleCuboid& p,
00220 pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
00221 pcl::KdTreeFLANN<pcl::PointXYZ>& tree,
00222 const Eigen::Vector3f& viewpoint,
00223 const std::vector<Polygon::Ptr>& polygons,
00224 const std::vector<float>& polygon_likelihood,
00225 const Config& config)
00226 {
00227 double range_likelihood = 1.0;
00228 if (config.use_range_likelihood) {
00229 range_likelihood = rangeLikelihood(p, cloud, polygons, config);
00230 }
00231
00232 if (range_likelihood == 0.0) {
00233 return range_likelihood;
00234 }
00235 else {
00236 return (range_likelihood * distanceFromPlaneBasedError(p, cloud, tree, viewpoint, config)
00237 * supportPlaneAngularLikelihood(p, polygons, config)
00238 * surfaceAreaLikelihood(p, config)
00239 * planeLikelihood(p, polygon_likelihood, config));
00240 }
00241 }
00242
00243 class PlaneSupportedCuboidEstimator: public jsk_topic_tools::DiagnosticNodelet
00244 {
00245 public:
00246 typedef pcl::tracking::ParticleCuboid Particle;
00247 typedef pcl::PointCloud<Particle> ParticleCloud;
00248 typedef boost::shared_ptr<PlaneSupportedCuboidEstimator> Ptr;
00249 typedef PlaneSupportedCuboidEstimatorConfig Config;
00250 typedef message_filters::sync_policies::ExactTime<
00251 jsk_recognition_msgs::PolygonArray,
00252 jsk_recognition_msgs::ModelCoefficientsArray> PolygonSyncPolicy;
00253
00254 PlaneSupportedCuboidEstimator(): DiagnosticNodelet("PlaneSupportedCuboidEstimator") {}
00255
00256 protected:
00257 virtual void onInit();
00258 virtual void subscribe();
00259 virtual void unsubscribe();
00260 virtual void polygonCallback(
00261 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00262 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coef_msg);
00263 virtual void cloudCallback(
00264 const sensor_msgs::PointCloud2::ConstPtr& msg);
00265 virtual pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr initParticles();
00266 virtual size_t chooseUniformRandomPlaneIndex(const std::vector<Polygon::Ptr>& polygons);
00267 virtual void configCallback(Config& config, uint32_t level);
00268
00269
00270 virtual pcl::tracking::ParticleCuboid sample(const pcl::tracking::ParticleCuboid& p);
00271
00272 virtual void likelihood(pcl::PointCloud<pcl::PointXYZ>::ConstPtr input,
00273 pcl::tracking::ParticleCuboid& p);
00274 virtual void publishHistogram(ParticleCloud::Ptr particles, int index,
00275 ros::Publisher& pub,
00276 const std_msgs::Header& header);
00277 virtual bool resetCallback(std_srvs::EmptyRequest& req,
00278 std_srvs::EmptyResponse& res);
00279
00284 virtual size_t getNearestPolygon(
00285 const Particle& p,
00286 const std::vector<ConvexPolygon::Ptr>& polygons);
00294 virtual void updateParticlePolygonRelationship(ParticleCloud::Ptr particles);
00295 boost::mutex mutex_;
00296 ros::Subscriber sub_cloud_;
00297 ros::Publisher pub_result_;
00298 ros::Publisher pub_particles_;
00299 ros::Publisher pub_candidate_cloud_;
00300 ros::Publisher pub_histogram_global_x_;
00301 ros::Publisher pub_histogram_global_y_;
00302 ros::Publisher pub_histogram_global_z_;
00303 ros::Publisher pub_histogram_global_roll_;
00304 ros::Publisher pub_histogram_global_pitch_;
00305 ros::Publisher pub_histogram_global_yaw_;
00306 ros::Publisher pub_histogram_dx_;
00307 ros::Publisher pub_histogram_dy_;
00308 ros::Publisher pub_histogram_dz_;
00309 ros::ServiceServer srv_reset_;
00310 message_filters::Subscriber<jsk_recognition_msgs::PolygonArray> sub_polygon_;
00311 message_filters::Subscriber<jsk_recognition_msgs::ModelCoefficientsArray> sub_coefficients_;
00312 boost::shared_ptr<message_filters::Synchronizer<PolygonSyncPolicy> > sync_polygon_;
00313 jsk_recognition_msgs::PolygonArray::ConstPtr latest_polygon_msg_;
00314 jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr latest_coefficients_msg_;
00315 pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_cloud_;
00316 boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00317
00318
00319 Config config_;
00320 double init_local_position_z_min_;
00321 double init_local_position_z_max_;
00322 bool use_init_world_position_z_model_;
00323 double init_world_position_z_min_;
00324 double init_world_position_z_max_;
00325 double init_local_orientation_roll_variance_;
00326 double init_local_orientation_pitch_variance_;
00327
00328 double init_local_orientation_yaw_mean_;
00329 double init_local_orientation_yaw_variance_;
00330
00331 double init_dx_mean_;
00332 double init_dx_variance_;
00333 double init_dy_mean_;
00334 double init_dy_variance_;
00335 double init_dz_mean_;
00336 double init_dz_variance_;
00337
00338 double step_x_variance_;
00339 double step_y_variance_;
00340 double step_z_variance_;
00341 double step_roll_variance_;
00342 double step_pitch_variance_;
00343 double step_yaw_variance_;
00344 double step_dx_variance_;
00345 double step_dy_variance_;
00346 double step_dz_variance_;
00347
00348 double min_dx_;
00349 double min_dy_;
00350 double min_dz_;
00351 bool use_init_polygon_likelihood_;
00352 int particle_num_;
00353 std::string sensor_frame_;
00354 boost::mt19937 random_generator_;
00355 tf::TransformListener* tf_;
00356 Eigen::Vector3f viewpoint_;
00357 bool support_plane_updated_;
00358 pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>::Ptr tracker_;
00359 std::vector<Polygon::Ptr> polygons_;
00360 pcl::KdTreeFLANN<pcl::PointXYZ> tree_;
00361 private:
00362 };
00363 }
00364
00365 #endif