motion_estimation_icp_prob_model.h

Go to the documentation of this file.

#ifndef RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H
#define RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H

#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/registration/transformation_estimation_svd.h>

#include "rgbdtools/types.h"
#include "rgbdtools/registration/motion_estimation.h"

namespace rgbdtools {

class MotionEstimationICPProbModel: public MotionEstimation
{
  public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MotionEstimationICPProbModel();
    
    virtual ~MotionEstimationICPProbModel();

    bool getMotionEstimationImpl(
      RGBDFrame& frame,
      const AffineTransform& prediction,
      AffineTransform& motion);
  
    int getModelSize() const { return model_size_; }

    PointCloudFeature::Ptr getModel()  { return model_ptr_; }
    Vector3fVector* getMeans()  { return &means_; }
    Matrix3fVector* getCovariances()  { return &covariances_; }
        
    void setMaxIterations(int max_iterations);
    void setMinCorrespondences(int min_correspondences);
    void setNNearestNeighbors(int n_nearest_neighbors);
    void setMaxModelSize(int max_model_size);
    void setTfEpsilonLinear(double tf_epsilon_linear);
    void setTfEpsilonAngular(double tf_epsilon_angular);     
    void setMaxAssociationDistMahalanobis(double max_assoc_dist_mah);
    void setMaxCorrespondenceDistEuclidean(double max_corresp_dist_eucl);
    void resetModel();
  private:

    // **** params

    int max_iterations_;        
    int min_correspondences_;   
    
    int n_nearest_neighbors_;  
    
    int max_model_size_;

    double tf_epsilon_linear_;     
    double tf_epsilon_angular_;    

    double max_assoc_dist_mah_;    

    double max_corresp_dist_eucl_; 
    

    double max_assoc_dist_mah_sq_;    
    
    double max_corresp_dist_eucl_sq_;

    // **** variables

    PointCloudFeature::Ptr model_ptr_; 
    int model_idx_;         
    int model_size_;        
    Vector3fVector means_;  
    Matrix3fVector covariances_; 

    KdTree model_tree_;     

    Matrix3f I_;            
    
    AffineTransform f2b_; 
    
    // ***** funtions
  
    bool alignICPEuclidean(
      const Vector3fVector& data_means,
      AffineTransform& correction);

    void getCorrespEuclidean(
      const PointCloudFeature& data_cloud,
      IntVector& data_indices,
      IntVector& model_indices); 
 
    bool getNNEuclidean(
      const PointFeature& data_point,
      int& eucl_nn_idx, double& eucl_dist_sq);

    bool getNNMahalanobis(
      const Vector3f& data_mean, const Matrix3f& data_cov,
      int& mah_nn_idx, double& mah_dist_sq,
      IntVector& indices, FloatVector& dists_sq);

    void initializeModelFromData(
      const Vector3fVector& data_means,
      const Matrix3fVector& data_covariances);
    
    void updateModelFromData(const Vector3fVector& data_means,
                             const Matrix3fVector& data_covariances);
  
    void addToModel(
      const Vector3f& data_mean,
      const Matrix3f& data_cov);

    bool saveModel(const std::string& filename);
};

} // namespace rgbdtools

#endif // RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H