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_recognition_utils/geo_util.h"
00047 #include "jsk_recognition_utils/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
00123 double cos_likelihood = (p.toEigenMatrix().rotation() * Eigen::Vector3f::UnitX()).dot(plane->getNormal());
00124
00125 return pow(std::abs(cos_likelihood),
00126 config.support_plane_angular_likelihood_weight_power);
00127 }
00128 else {
00129 return 1.0;
00130 }
00131 }
00132
00133
00134 template <class Config>
00135 double surfaceAreaLikelihood(
00136 const pcl::tracking::ParticleCuboid& p,
00137 const Config& config)
00138 {
00139 if (config.use_surface_area_likelihood) {
00140 double v = p.area();
00141 return 1.0 / (1.0 + pow(v, config.surface_area_error_power));
00142 }
00143 else {
00144 return 1.0;
00145 }
00146 }
00147
00148 template <class Config>
00149 double distanceFromPlaneBasedError(
00150 const pcl::tracking::ParticleCuboid& p,
00151 pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
00152 pcl::KdTreeFLANN<pcl::PointXYZ>& tree,
00153 const Eigen::Vector3f& viewpoint,
00154 const Config& config)
00155 {
00156 Eigen::Affine3f pose = p.toEigenMatrix();
00157 Eigen::Affine3f pose_inv = pose.inverse();
00158 const Eigen::Vector3f local_vp = pose_inv * viewpoint;
00159 std::vector<int> visible_faces = p.visibleFaceIndices(local_vp);
00160 double r = sqrt(p.dx /2 * p.dx/2 + p.dy/2 * p.dy/2 + p.dz/2 * p.dz/2);
00161 std::vector<int> candidate_point_indices;
00162 std::vector<float> candidate_point_distances;
00163 pcl::PointXYZ xyz_point;
00164 xyz_point.getVector3fMap() = p.getVector3fMap();
00165 size_t inliers = 0;
00166
00167 tree.radiusSearch(xyz_point, r + config.outlier_distance, candidate_point_indices, candidate_point_distances);
00168 if (candidate_point_indices.size() < config.min_inliers) {
00169 return 0;
00170 }
00171 else {
00172
00173 double error = 0.0;
00174
00175 jsk_recognition_utils::Cube::Ptr cube = p.toCube();
00176 std::vector<Polygon::Ptr> faces = cube->faces();
00177 for (size_t i = 0; i < candidate_point_indices.size(); i++) {
00178 int index = candidate_point_indices[i];
00179 Eigen::Vector3f v = cloud->points[index].getVector3fMap();
00180 if (config.use_occlusion_likelihood) {
00181 double d = p.distanceNearestToPlaneWithOcclusion(v, visible_faces, faces);
00182 if (d <= config.outlier_distance) {
00183
00184 error += pow(d, config.plane_distance_error_power);
00185 ++inliers;
00186 }
00187 }
00188 else {
00189 Eigen::Vector3f local_v = pose_inv * v;
00190 double d = p.signedDistanceToPlane(local_v, p.nearestPlaneIndex(local_v));
00191 if (config.use_inside_points_distance_zero) {
00192 if (d < 0) {
00193 d = 0;
00194 }
00195 }
00196 else {
00197 d = std::abs(d);
00198 }
00199 if (d <= config.outlier_distance) {
00200
00201 error += pow(d, config.plane_distance_error_power);
00202 ++inliers;
00203 }
00204 }
00205 }
00206
00207
00208 size_t expected_num = p.volume() / config.expected_density / config.expected_density / config.expected_density;
00209
00210 if (inliers < config.min_inliers) {
00211 return 0;
00212 }
00213 else {
00214 double non_inlier_value = 1 / (1 + error / inliers);
00215 if (config.use_inliers) {
00216
00217
00218
00219 double inlier_likelihood = 1 / (1 + pow(expected_num - inliers, config.inliers_power));
00220
00221 return non_inlier_value * inlier_likelihood;
00222 }
00223 else {
00224 return non_inlier_value;
00225 }
00226 }
00227 }
00228 }
00229
00230 template <class Config>
00231 double planeLikelihood(const pcl::tracking::ParticleCuboid& p,
00232 const std::vector<float>& polygon_likelihood,
00233 const Config& config)
00234 {
00235 if (config.use_polygon_likelihood) {
00236 return polygon_likelihood[p.plane_index];
00237 }
00238 else {
00239 return 1.0;
00240 }
00241 }
00242
00243 template <class Config>
00244 double computeLikelihood(const pcl::tracking::ParticleCuboid& p,
00245 pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
00246 pcl::KdTreeFLANN<pcl::PointXYZ>& tree,
00247 const Eigen::Vector3f& viewpoint,
00248 const std::vector<Polygon::Ptr>& polygons,
00249 const std::vector<float>& polygon_likelihood,
00250 const Config& config)
00251 {
00252 double range_likelihood = 1.0;
00253 if (config.use_range_likelihood) {
00254 range_likelihood = rangeLikelihood(p, cloud, polygons, config);
00255 }
00256
00257 if (range_likelihood == 0.0) {
00258 return range_likelihood;
00259 }
00260 else {
00261 return (range_likelihood * distanceFromPlaneBasedError(p, cloud, tree, viewpoint, config)
00262 * supportPlaneAngularLikelihood(p, polygons, config)
00263 * surfaceAreaLikelihood(p, config)
00264 * planeLikelihood(p, polygon_likelihood, config));
00265 }
00266 }
00267
00268 class PlaneSupportedCuboidEstimator: public jsk_topic_tools::DiagnosticNodelet
00269 {
00270 public:
00271 typedef pcl::tracking::ParticleCuboid Particle;
00272 typedef pcl::PointCloud<Particle> ParticleCloud;
00273 typedef boost::shared_ptr<PlaneSupportedCuboidEstimator> Ptr;
00274 typedef PlaneSupportedCuboidEstimatorConfig Config;
00275 typedef message_filters::sync_policies::ExactTime<
00276 jsk_recognition_msgs::PolygonArray,
00277 jsk_recognition_msgs::ModelCoefficientsArray> PolygonSyncPolicy;
00278
00279 PlaneSupportedCuboidEstimator(): DiagnosticNodelet("PlaneSupportedCuboidEstimator") {}
00280
00281 protected:
00282 virtual void onInit();
00283 virtual void subscribe();
00284 virtual void unsubscribe();
00285 virtual void polygonCallback(
00286 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00287 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coef_msg);
00288 virtual void cloudCallback(
00289 const sensor_msgs::PointCloud2::ConstPtr& msg);
00290 virtual void estimate(
00291 const sensor_msgs::PointCloud2::ConstPtr& msg);
00292 virtual void fastCloudCallback(
00293 const sensor_msgs::PointCloud2::ConstPtr& msg);
00294 virtual pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr initParticles();
00295 virtual size_t chooseUniformRandomPlaneIndex(const std::vector<Polygon::Ptr>& polygons);
00296 virtual void configCallback(Config& config, uint32_t level);
00297
00298
00299 virtual pcl::tracking::ParticleCuboid sample(const pcl::tracking::ParticleCuboid& p);
00300
00301 virtual void likelihood(pcl::PointCloud<pcl::PointXYZ>::ConstPtr input,
00302 pcl::tracking::ParticleCuboid& p);
00303 virtual void publishHistogram(ParticleCloud::Ptr particles, int index,
00304 ros::Publisher& pub,
00305 const std_msgs::Header& header);
00306 virtual bool resetCallback(std_srvs::EmptyRequest& req,
00307 std_srvs::EmptyResponse& res);
00308
00313 virtual size_t getNearestPolygon(
00314 const Particle& p,
00315 const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& polygons);
00323 virtual void updateParticlePolygonRelationship(ParticleCloud::Ptr particles);
00324 boost::mutex mutex_;
00325 ros::Subscriber sub_cloud_;
00326 ros::Subscriber sub_fast_cloud_;
00327 ros::Publisher pub_result_;
00328 ros::Publisher pub_particles_;
00329 ros::Publisher pub_candidate_cloud_;
00330 ros::Publisher pub_histogram_global_x_;
00331 ros::Publisher pub_histogram_global_y_;
00332 ros::Publisher pub_histogram_global_z_;
00333 ros::Publisher pub_histogram_global_roll_;
00334 ros::Publisher pub_histogram_global_pitch_;
00335 ros::Publisher pub_histogram_global_yaw_;
00336 ros::Publisher pub_histogram_dx_;
00337 ros::Publisher pub_histogram_dy_;
00338 ros::Publisher pub_histogram_dz_;
00339 ros::Publisher pub_result_pose_;
00340 ros::ServiceServer srv_reset_;
00341 message_filters::Subscriber<jsk_recognition_msgs::PolygonArray> sub_polygon_;
00342 message_filters::Subscriber<jsk_recognition_msgs::ModelCoefficientsArray> sub_coefficients_;
00343 boost::shared_ptr<message_filters::Synchronizer<PolygonSyncPolicy> > sync_polygon_;
00344 jsk_recognition_msgs::PolygonArray::ConstPtr latest_polygon_msg_;
00345 jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr latest_coefficients_msg_;
00346 pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_cloud_;
00347 boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00348
00349
00350 Config config_;
00351 double init_local_position_z_min_;
00352 double init_local_position_z_max_;
00353 bool use_init_world_position_z_model_;
00354 double init_world_position_z_min_;
00355 double init_world_position_z_max_;
00356 double init_local_orientation_roll_mean_;
00357 double init_local_orientation_roll_variance_;
00358 double init_local_orientation_pitch_mean_;
00359 double init_local_orientation_pitch_variance_;
00360 double init_local_orientation_yaw_mean_;
00361 double init_local_orientation_yaw_variance_;
00362 bool use_global_init_yaw_;
00363 double init_global_orientation_yaw_mean_;
00364 double init_global_orientation_yaw_variance_;
00365
00366 bool disable_init_roll_;
00367 bool disable_init_pitch_;
00368
00369 double init_dx_mean_;
00370 double init_dx_variance_;
00371 double init_dy_mean_;
00372 double init_dy_variance_;
00373 double init_dz_mean_;
00374 double init_dz_variance_;
00375
00376 double step_x_variance_;
00377 double step_y_variance_;
00378 double step_z_variance_;
00379 double step_roll_variance_;
00380 double step_pitch_variance_;
00381 double step_yaw_variance_;
00382 double step_dx_variance_;
00383 double step_dy_variance_;
00384 double step_dz_variance_;
00385
00386 double min_dx_;
00387 double min_dy_;
00388 double min_dz_;
00389 double fast_cloud_threshold_;
00390 bool use_init_polygon_likelihood_;
00391 int particle_num_;
00392 std::string sensor_frame_;
00393 boost::mt19937 random_generator_;
00394 tf::TransformListener* tf_;
00395 Eigen::Vector3f viewpoint_;
00396 bool support_plane_updated_;
00397 pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>::Ptr tracker_;
00398 std::vector<Polygon::Ptr> polygons_;
00399 pcl::KdTreeFLANN<pcl::PointXYZ> tree_;
00400 private:
00401 };
00402 }
00403
00404 #endif