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
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 #ifndef SHAPE_RECONSTRUCTION_H_
00049 #define SHAPE_RECONSTRUCTION_H_
00050
00051 #include <omip_common/OMIPTypeDefs.h>
00052
00053
00054 #include <opencv2/core/core.hpp>
00055 #include <camera_info_manager/camera_info_manager.h>
00056
00057 #include <pcl/range_image/range_image_planar.h>
00058 #include <pcl/filters/extract_indices.h>
00059 #include <pcl/common/transforms.h>
00060 #include <pcl/search/search.h>
00061 #include <pcl/search/organized.h>
00062 #include <pcl/search/kdtree.h>
00063 #include <pcl/filters/voxel_grid.h>
00064 #include <pcl/kdtree/kdtree_flann.h>
00065
00066 #include <omip_common/OMIPUtils.h>
00067
00068 #include <pcl/filters/radius_outlier_removal.h>
00069
00070 #include <pcl/filters/approximate_voxel_grid.h>
00071
00072 #include <pcl/features/normal_3d_omp.h>
00073 #include <pcl/point_types.h>
00074 #include <pcl/io/pcd_io.h>
00075 #include <pcl/kdtree/kdtree_flann.h>
00076 #include <pcl/features/normal_3d.h>
00077 #include <pcl/surface/gp3.h>
00078 #include <pcl/io/vtk_lib_io.h>
00079 #include <pcl/segmentation/extract_clusters.h>
00080
00081 #include <pcl/segmentation/supervoxel_clustering.h>
00082
00083 #include <boost/circular_buffer.hpp>
00084
00085 #include "shape_reconstruction/SRUtils.h"
00086
00087 #include <opencv2/highgui/highgui.hpp>
00088
00089 #include <rosbag/bag.h>
00090
00091 #include <omip_msgs/ShapeModel.h>
00092 #include <omip_msgs/ShapeModels.h>
00093
00094 #include <unordered_set>
00095
00096 namespace omip
00097 {
00098
00099 struct FrameForSegmentation
00100 {
00101 ros::Time _time;
00102 SRPointCloud::Ptr _pc;
00103 SRPointCloud::Ptr _pc_moving_pts;
00104 SRPointCloud::Ptr _pc_moving_pts_transf;
00105 SRPointCloud::Ptr _pc_without_nans;
00106 std::vector<int> _not_nan_indices;
00107 sensor_msgs::Image _rgb;
00108 cv_bridge::CvImagePtr _dm;
00109 cv::Mat _dm_filled;
00110 geometry_msgs::TwistWithCovariance _transformation;
00111
00112 FrameForSegmentation()
00113 {
00114 this->_time.fromSec(0.0);
00115 this->_pc = SRPointCloud::Ptr(new SRPointCloud);
00116 this->_pc_moving_pts = SRPointCloud::Ptr(new SRPointCloud);
00117 this->_pc_moving_pts_transf = SRPointCloud::Ptr(new SRPointCloud);
00118 this->_pc_without_nans = SRPointCloud::Ptr(new SRPointCloud);
00119 this->_rgb = sensor_msgs::Image();
00120 this->_dm = cv_bridge::CvImagePtr(new cv_bridge::CvImage());
00121 this->_dm->encoding = std::string("32FC1");
00122 this->_dm->header.frame_id = "camera_rgb_optical_frame";
00123 this->_dm->image = cv::Mat(IMG_HEIGHT, IMG_WIDTH, CV_32FC1);
00124 this->_dm_filled = cv::Mat(IMG_HEIGHT, IMG_WIDTH, CV_32FC1);
00125 this->_transformation = geometry_msgs::TwistWithCovariance();
00126 }
00127
00128 void reset() {
00129 this->_time.fromSec(0.0);
00130 this->_pc = SRPointCloud::Ptr(new SRPointCloud);
00131 this->_pc_moving_pts = SRPointCloud::Ptr(new SRPointCloud);
00132 this->_pc_moving_pts_transf = SRPointCloud::Ptr(new SRPointCloud);
00133 this->_pc_without_nans = SRPointCloud::Ptr(new SRPointCloud);
00134 this->_rgb = sensor_msgs::Image();
00135 this->_dm = cv_bridge::CvImagePtr(new cv_bridge::CvImage());
00136 this->_dm->encoding = std::string("32FC1");
00137 this->_dm->header.frame_id = "camera_rgb_optical_frame";
00138 this->_dm->image = cv::Mat(IMG_HEIGHT, IMG_WIDTH, CV_32FC1);
00139 this->_dm_filled = cv::Mat(IMG_HEIGHT, IMG_WIDTH, CV_32FC1);
00140 this->_transformation = geometry_msgs::TwistWithCovariance();
00141 }
00142
00143
00144 FrameForSegmentation clone() const {
00145 FrameForSegmentation new_ffs;
00146 new_ffs._time = this->_time;
00147 new_ffs._pc.reset(new SRPointCloud(*this->_pc));
00148 new_ffs._pc_moving_pts.reset(new SRPointCloud(*this->_pc_moving_pts));
00149 new_ffs._pc_moving_pts_transf.reset(new SRPointCloud(*this->_pc_moving_pts_transf));
00150 new_ffs._pc_without_nans.reset(new SRPointCloud(*this->_pc_without_nans));
00151 new_ffs._not_nan_indices = _not_nan_indices;
00152 new_ffs._rgb = _rgb;
00153 * (new_ffs._dm) = *(_dm);
00154 new_ffs._dm_filled = _dm_filled.clone();
00155 new_ffs._transformation = _transformation;
00156 return new_ffs;
00157 }
00158 };
00159
00160
00161 class ShapeReconstruction;
00162 typedef boost::shared_ptr<ShapeReconstruction> ShapeReconstructionPtr;
00163
00164 class ShapeReconstruction
00165 {
00166 public:
00167 ShapeReconstruction();
00168
00169 virtual void setInitialFullRGBDPCAndRBT(const SRPointCloud::Ptr &initial_pc_msg,
00170 const geometry_msgs::TwistWithCovariance &rb_transformation_initial);
00171
00172 virtual void initialize();
00173
00174 ShapeReconstruction(const ShapeReconstruction &sr);
00175
00176 virtual ~ShapeReconstruction();
00177
00178 virtual void setCameraInfo(const sensor_msgs::CameraInfo& camera_info);
00179
00180 virtual void setFullRGBDPCandRBT(const SRPointCloud::Ptr &pc_msg,
00181 const geometry_msgs::TwistWithCovariance &rb_transformation);
00182
00183 virtual void generateMesh();
00184
00185 virtual void getShapeModel(omip_msgs::ShapeModelsPtr shapes);
00186
00187 virtual void PublishMovedModelAndSegment(const ros::Time current_time,
00188 const geometry_msgs::TwistWithCovariance &rb_transformation,
00189 rosbag::Bag& bag,
00190 bool bagOpen);
00191
00192 virtual void RemoveInconsistentPoints(const SRPointCloud::Ptr &pc_msg,
00193 const geometry_msgs::TwistWithCovariance &rb_transformation);
00194
00195 omip::RB_id_t getId() const
00196 {
00197 return this->_rb_id;
00198 }
00199
00200 SRPointCloud::Ptr getCurrentPointCloud() const
00201 {
00202 return this->_current_ffs._pc;
00203 }
00204
00205 SRPointCloud::Ptr getRigidBodyShapeExtDandC() const
00206 {
00207 return this->_rb_shape;
00208 }
00209
00210 SRPointCloud::Ptr getMovedRigidBodyShapeExtDandC() const
00211 {
00212 return this->_rb_shape_in_current_frame;
00213 }
00214
00215 virtual void setDetectStaticEnvironment(bool b)
00216 {
00217 _detect_static_environment = b;
00218 }
00219
00220 virtual void setAccumulateChangeCandidates(bool b)
00221 {
00222 _accumulate_change_candidates = b;
00223 }
00224
00225 virtual void setMinDepthChange(double b)
00226 {
00227 _min_depth_change = b;
00228 }
00229
00230 virtual void setRemoveInconsistentPoints(double b)
00231 {
00232 _remove_inconsistent_points = b;
00233 }
00234
00235 virtual void setMinColorChange(int min_color_change)
00236 {
00237 _min_color_change = min_color_change;
00238 }
00239
00240 virtual void setKNNMinRadius(double knn_min_radius)
00241 {
00242 _knn_min_radius = knn_min_radius;
00243 }
00244
00245 virtual void setSVMinNumberModelPixels(int min_number_model_pixels_in_sv)
00246 {
00247 _min_number_model_pixels_in_sv = min_number_model_pixels_in_sv;
00248 }
00249
00250 virtual void setSuperVoxelizerPtr(boost::shared_ptr<pcl::SupervoxelClustering<SRPoint> >& supervoxelizer)
00251 {
00252 this->_supervoxelizer_ptr_ptr = &supervoxelizer;
00253 }
00254
00255 virtual void setSuperVoxelClustersPtr(std::map <uint32_t, pcl::Supervoxel<SRPoint>::Ptr >& supervoxel_clusters)
00256 {
00257 this->_supervoxel_clusters_ptr = &supervoxel_clusters;
00258 }
00259
00260 virtual void setExtendToNeighborSV(bool extend_to_neighbor_sv)
00261 {
00262 this->_extend_to_neighbor_sv = extend_to_neighbor_sv;
00263 }
00264
00265 virtual void setSimilarityInH(double similarity_in_h)
00266 {
00267 this->_similarity_in_h = similarity_in_h;
00268 }
00269
00270 virtual void setSimilarityInNormal(double similarity_in_normal)
00271 {
00272 this->_similarity_in_normal = similarity_in_normal;
00273 }
00274
00275 virtual void setSupportingFeatures(pcl::PointCloud<pcl::PointXYZL>::Ptr supporting_features)
00276 {
00277 this->_supporting_features = supporting_features;
00278 }
00279
00280 virtual void useClusteredFeatures(bool use_clustered_features)
00281 {
00282 this->_use_clustered_features = use_clustered_features;
00283 }
00284
00285 virtual void setDepthFilling(bool depth_filling)
00286 {
00287 this->_depth_filling = depth_filling;
00288 }
00289
00290 virtual void setColorChangesErodeSize(double color_changes_erode_size)
00291 {
00292 this->_color_changes_erode_size = color_changes_erode_size;
00293 }
00294
00295 virtual void setToInitial(bool to_initial)
00296 {
00297 this->_to_initial = to_initial;
00298 }
00299
00300 virtual void setRecordVideos(bool record_videos)
00301 {
00302 this->_record_videos = record_videos;
00303 }
00304
00305 virtual void setLiveStream(bool live_stream)
00306 {
00307 this->_live_stream = live_stream;
00308 }
00309
00310 virtual void setRBId(omip::RB_id_t rb_id)
00311 {
00312 this->_rb_id = rb_id;
00313 }
00314
00315 virtual void setApproxVoxelGridLeafSize(double leaf_size)
00316 {
00317 this->_leaf_size = leaf_size;
00318 }
00319
00320 virtual void setRadiusOutRemovalSearch(double ror_radius_search)
00321 {
00322 this->_ror_radius_search = ror_radius_search;
00323 }
00324
00325 virtual void setRadiusOutRemovalMinNeighbors(int ror_min_neighbors)
00326 {
00327 this->_ror_min_neighbors = ror_min_neighbors;
00328 }
00329
00330 ShapeReconstructionPtr clone() const
00331 {
00332 return (ShapeReconstructionPtr(doClone()));
00333 }
00334
00335 void setEstimateSV(bool estimate_sv)
00336 {
00337 _estimate_supervoxels = estimate_sv;
00338 }
00339
00340 protected:
00341
00343 virtual ShapeReconstruction* doClone() const
00344 {
00345 return (new ShapeReconstruction(*this));
00346 }
00347
00348 virtual void _DetectImageLocationsWhereDepthChanges();
00349
00350 virtual void _DetectImageLocationsWhereColorChanges();
00351
00352 virtual void _TestMotionCoherencyOfPointsInImageLocationsWhereDepthChanged();
00353
00354 virtual void _TestMotionCoherencyOfPointsInImageLocationsWhereColorChanged();
00355
00356 virtual void _FindCandidatesInPreviousPC(FrameForSegmentation& _previous_ffs,
00357 const FrameForSegmentation& _current_ffs,
00358 pcl::PointIndices::Ptr& _moving_pts_of_previous_idx_depth,
00359 Eigen::Matrix4d& _current_to_previous_HTransform,
00360 std::vector<std::vector<int > >& _current_k_indices_depth,
00361 std::vector<std::vector<float > >& _current_sqrt_dist_depth);
00362
00363 virtual void _FindCandidatesInCurrentPC(const FrameForSegmentation& _previous_ffs,
00364 FrameForSegmentation& _current_ffs,
00365 pcl::PointIndices::Ptr& _moving_pts_of_current_idx_depth,
00366 Eigen::Matrix4d& _previous_to_current_HTransform,
00367 std::vector<std::vector<int > >& _previous_k_indices_depth,
00368 std::vector<std::vector<float > >& _previous_sqrt_dist_depth);
00369
00370 virtual void _MergeValidPointsIntoModels();
00371
00372 virtual void _MergeValidPointsIntoModels(const std::string& logger_name,
00373 omip::SRPointCloud::Ptr& points_of_current_in_origin,
00374 omip::SRPointCloud::Ptr& points_of_previous_in_origin,
00375 omip::SRPointCloud::Ptr& rb_model,
00376 const std::vector<std::vector<int > >& current_k_indices,
00377 const std::vector<std::vector<int > >& previous_k_indices);
00378
00379
00380 virtual void _RemoveInconsistentPointsFromModelsAndExtendToRegions();
00381
00382 virtual void _RemoveInconsistentPointsFromRBModel(const std::string method,
00383 const Eigen::Matrix4d& HTransform,
00384 SRPointCloud::Ptr& rb_shape_depth,
00385 cv::Mat& current_dm,
00386 SRPointCloud::Ptr current_pc,
00387 SRPointCloud::Ptr& rb_segment_depth);
00388
00389 virtual void _FindInconsistentPoints(const omip::SRPointCloud::Ptr& pc_source,
00390 const cv::Mat & dm_true,
00391 pcl::PointIndicesPtr& indices_to_remove,
00392 pcl::PointIndicesPtr& indices_matching_in_true,
00393 pcl::PointIndicesPtr& indices_matching_in_dm,
00394 const double min_depth_error=0.03);
00395
00396 virtual void _ExtendPointsToRegions();
00397
00398 virtual void _ExtendToNeighborSV(std::multimap<uint32_t, uint32_t>& supervoxel_adjacency,
00399 std::vector<uint32_t>& labels_extension);
00400
00401 virtual void _FilterModel();
00402
00403 virtual void _EstimateTransformations();
00404
00405 virtual void _GenerateMesh(const omip::SRPointCloud::Ptr& pc_source,
00406 std::string shape_file_prefix);
00407
00408 void growSVRecursively(uint32_t label_sv_seed,
00409 std::vector<uint32_t>& labels_extension,
00410 std::multimap<uint32_t, uint32_t>& supervoxel_adjacency,
00411 std::vector<uint32_t>& sv_containing_supporting_features,
00412 int& distance_to_feature_sv,
00413 std::map<uint32_t, std::unordered_set<int> >& labels_of_segments_to_extend);
00414
00415
00416 omip::RB_id_t _rb_id;
00417
00418 SRPointCloud::Ptr _rb_shape;
00419 SRPointCloud::Ptr _rb_shape_in_current_frame;
00420 SRPointCloud::Ptr _rb_segment;
00421
00422 bool _to_initial;
00423 bool _depth_filling;
00424 bool _detect_static_environment;
00425 bool _accumulate_change_candidates;
00426 bool _remove_inconsistent_points;
00427 bool _record_videos;
00428 bool _extend_to_neighbor_sv;
00429 bool _use_clustered_features;
00430 bool _live_stream;
00431 bool _estimate_supervoxels;
00432
00433 double _min_depth_change;
00434 int _min_color_change;
00435 double _knn_min_radius;
00436 int _min_number_model_pixels_in_sv;
00437 double _color_changes_erode_size;
00438 double _timestamp_color;
00439 double _similarity_in_h;
00440 double _similarity_in_normal;
00441 int _t;
00442
00443 FrameForSegmentation _initial_ffs;
00444 FrameForSegmentation _previous_ffs;
00445 FrameForSegmentation _current_ffs;
00446
00447 pcl::RangeImagePlanar::Ptr _rip_temp;
00448
00449 cv::Mat _acc_candidates_of_current;
00450 cv::Mat _difference_in_depth;
00451 cv::Mat _candidates_of_current;
00452 cv::Mat _candidates_of_previous;
00453 cv::Mat _candidates_of_current_8u;
00454 cv::Mat _candidates_of_previous_8u;
00455 cv::Mat _previous_depth_mask, _current_depth_mask;
00456
00457 cv::Mat _difference_in_depth_moved;
00458
00459 pcl::PointIndices::Ptr _moving_pts_of_current_idx_depth;
00460 pcl::PointIndices::Ptr _moving_pts_of_previous_idx_depth;
00461
00462 pcl::PointIndices::Ptr _moving_pts_of_current_idx_color;
00463 pcl::PointIndices::Ptr _moving_pts_of_previous_idx_color;
00464
00465 pcl::PointIndices::Ptr _occluded_pts_of_current_idx;
00466 pcl::PointIndices::Ptr _out_of_view_pts_of_current_idx;
00467
00468 Eigen::Twistd _previous_twist;
00469 Eigen::Matrix4d _previous_HTransform;
00470 Eigen::Matrix4d _previous_HTransform_inv;
00471
00472 Eigen::Twistd _current_twist;
00473 Eigen::Matrix4d _current_HTransform;
00474 Eigen::Matrix4d _current_HTransform_inv;
00475
00476 Eigen::Matrix4d _current_to_previous_HTransform;
00477 Eigen::Matrix4d _previous_to_current_HTransform;
00478
00479 std::vector<std::vector<int > > _current_k_indices;
00480 std::vector<std::vector<int > > _previous_k_indices;
00481 std::vector<std::vector<int > > _previous_k_indices_previous;
00482 std::vector<std::vector<float > > _sqrt_dist;
00483
00484
00485 pcl::ExtractIndices<SRPoint> _extractor;
00486
00487 pcl::search::Search<SRPoint>::Ptr _knn;
00488
00489 ros::Publisher _rb_shape_pub;
00490 ros::Publisher _occlusions_pub;
00491 ros::Publisher _occlusions_transformed_pub;
00492 ros::Publisher _rb_segment_pub;
00493
00494 ros::NodeHandle _node_handle;
00495
00496 SRPointCloud::Ptr _candidates;
00497 SRPointCloud::Ptr _candidates_in_current;
00498
00499
00500 pcl::RadiusOutlierRemoval<SRPoint> _radius_outlier_removal;
00501 int _ror_min_neighbors;
00502 double _ror_radius_search;
00503
00504
00505 pcl::ApproximateVoxelGrid<SRPoint> _approximate_voxel_grid_filter;
00506 double _leaf_size;
00507
00508
00509 pcl::NormalEstimationOMP<omip::SRPoint, pcl::Normal>::Ptr _rb_normal_estimator_ptr;
00510 pcl::PointCloud<pcl::Normal>::Ptr _rb_estimated_normals_pc_ptr;
00511 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr _rb_position_color_and_normals_pc_ptr;
00512 pcl::search::KdTree<omip::SRPoint>::Ptr _rb_tree_for_normal_estimation_ptr;
00513 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr _rb_tree_for_triangulation_ptr;
00514 pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>::Ptr _rb_greedy_projection_triangulator_ptr;
00515 pcl::PolygonMesh::Ptr _rb_triangulated_mesh_ptr;
00516 vtkSmartPointer<vtkPolyData> _rb_polygon_data_ptr;
00517 vtkSmartPointer<vtkSTLWriter> _rb_polygon_writer_ptr;
00518
00519
00520 rosbag::Bag _videos;
00521
00522
00523 boost::shared_ptr<pcl::SupervoxelClustering<SRPoint> > * _supervoxelizer_ptr_ptr;
00524 std::map <uint32_t, pcl::Supervoxel<SRPoint>::Ptr >* _supervoxel_clusters_ptr;
00525
00526
00527 sensor_msgs::CameraInfo _ci;
00528
00529 pcl::PointCloud<pcl::PointXYZL>::Ptr _supporting_features;
00530 };
00531 }
00532
00533 #endif