Go to the documentation of this file.00001
00024 #ifndef RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H
00025 #define RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H
00026
00027 #include <pcl/io/pcd_io.h>
00028 #include <pcl/kdtree/kdtree.h>
00029 #include <pcl/registration/transformation_estimation_svd.h>
00030
00031 #include "rgbdtools/types.h"
00032 #include "rgbdtools/registration/motion_estimation.h"
00033
00034 namespace rgbdtools {
00035
00042 class MotionEstimationICPProbModel: public MotionEstimation
00043 {
00044 public:
00045
00046 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00047
00052 MotionEstimationICPProbModel();
00053
00056 virtual ~MotionEstimationICPProbModel();
00057
00065 bool getMotionEstimationImpl(
00066 RGBDFrame& frame,
00067 const AffineTransform& prediction,
00068 AffineTransform& motion);
00069
00073 int getModelSize() const { return model_size_; }
00074
00075 PointCloudFeature::Ptr getModel() { return model_ptr_; }
00076 Vector3fVector* getMeans() { return &means_; }
00077 Matrix3fVector* getCovariances() { return &covariances_; }
00078
00079 void setMaxIterations(int max_iterations);
00080 void setMinCorrespondences(int min_correspondences);
00081 void setNNearestNeighbors(int n_nearest_neighbors);
00082 void setMaxModelSize(int max_model_size);
00083 void setTfEpsilonLinear(double tf_epsilon_linear);
00084 void setTfEpsilonAngular(double tf_epsilon_angular);
00085 void setMaxAssociationDistMahalanobis(double max_assoc_dist_mah);
00086 void setMaxCorrespondenceDistEuclidean(double max_corresp_dist_eucl);
00087 void resetModel();
00088 private:
00089
00090
00091
00092 int max_iterations_;
00093 int min_correspondences_;
00094
00098 int n_nearest_neighbors_;
00099
00104 int max_model_size_;
00105
00106 double tf_epsilon_linear_;
00107 double tf_epsilon_angular_;
00108
00112 double max_assoc_dist_mah_;
00113
00116 double max_corresp_dist_eucl_;
00117
00118
00122 double max_assoc_dist_mah_sq_;
00123
00126 double max_corresp_dist_eucl_sq_;
00127
00128
00129
00130 PointCloudFeature::Ptr model_ptr_;
00131 int model_idx_;
00132 int model_size_;
00133 Vector3fVector means_;
00134 Matrix3fVector covariances_;
00135
00136 KdTree model_tree_;
00137
00138 Matrix3f I_;
00139
00140 AffineTransform f2b_;
00141
00142
00143
00150 bool alignICPEuclidean(
00151 const Vector3fVector& data_means,
00152 AffineTransform& correction);
00153
00159 void getCorrespEuclidean(
00160 const PointCloudFeature& data_cloud,
00161 IntVector& data_indices,
00162 IntVector& model_indices);
00163
00171 bool getNNEuclidean(
00172 const PointFeature& data_point,
00173 int& eucl_nn_idx, double& eucl_dist_sq);
00174
00190 bool getNNMahalanobis(
00191 const Vector3f& data_mean, const Matrix3f& data_cov,
00192 int& mah_nn_idx, double& mah_dist_sq,
00193 IntVector& indices, FloatVector& dists_sq);
00194
00200 void initializeModelFromData(
00201 const Vector3fVector& data_means,
00202 const Matrix3fVector& data_covariances);
00203
00216 void updateModelFromData(const Vector3fVector& data_means,
00217 const Matrix3fVector& data_covariances);
00218
00228 void addToModel(
00229 const Vector3f& data_mean,
00230 const Matrix3f& data_cov);
00231
00232 bool saveModel(const std::string& filename);
00233 };
00234
00235 }
00236
00237 #endif // RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H
00238