motion_estimation_icp.h
Go to the documentation of this file.
00001 
00024 #ifndef CCNY_RGBD_MOTION_ESTIMATION_ICP_H
00025 #define CCNY_RGBD_MOTION_ESTIMATION_ICP_H
00026 
00027 #include <tf/transform_datatypes.h>
00028 #include <pcl/registration/transformation_estimation_svd.h>
00029 #include <pcl_ros/point_cloud.h>
00030 #include <pcl_ros/transforms.h>
00031 
00032 #include "ccny_rgbd/types.h"
00033 #include "ccny_rgbd/structures/feature_history.h"
00034 #include "ccny_rgbd/registration/motion_estimation.h"
00035 
00036 namespace ccny_rgbd {
00037   
00043 class MotionEstimationICP: public MotionEstimation
00044 {
00045   public:
00046 
00047     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00048 
00053     MotionEstimationICP(const ros::NodeHandle& nh, 
00054                         const ros::NodeHandle& nh_private);
00055         
00058     virtual ~MotionEstimationICP();
00059 
00067     bool getMotionEstimationImpl(
00068       RGBDFrame& frame,
00069       const tf::Transform& prediction,
00070       tf::Transform& motion);
00071   
00075     int getModelSize() const { return model_ptr_->points.size(); }
00076     
00077   private:
00078 
00079     // **** ros-related
00080     
00081     ros::Publisher model_publisher_;  
00082     
00083     // **** parameters
00084   
00085     std::string fixed_frame_;      
00086     std::string base_frame_;       
00087 
00088     int max_iterations_;           
00089     int min_correspondences_;      
00090     double tf_epsilon_linear_;     
00091     double tf_epsilon_angular_;    
00092     double max_corresp_dist_eucl_; 
00093     
00094     bool publish_model_;           
00095     
00096     double max_corresp_dist_eucl_sq_; 
00097     
00098     // **** variables
00099 
00100     PointCloudFeature::Ptr model_ptr_; 
00101     KdTree model_tree_;                
00102 
00103     FeatureHistory feature_history_; 
00104 
00105     tf::Transform f2b_; 
00106     
00113     bool alignICPEuclidean(
00114       const Vector3fVector& data_means,
00115       tf::Transform& correction);
00116     
00122     void getCorrespEuclidean(
00123       const PointCloudFeature& data_cloud,
00124       IntVector& data_indices,
00125       IntVector& model_indices);
00126     
00134     bool getNNEuclidean(
00135       const PointFeature& data_point,
00136       int& eucl_nn_idx, double& eucl_dist_sq);
00137 };
00138 
00139 } //namespace ccny_rgbd
00140 
00141 #endif // CCNY_RGBD_MOTION_ESTIMATION_ICP_H
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends


ccny_rgbd
Author(s): Ivan Dryanovski
autogenerated on Fri Apr 12 2013 20:38:48