Go to the documentation of this file.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 #ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_H_
00041 #define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_H_
00042
00043 #include <pcl/point_representation.h>
00044 #include <pcl/search/kdtree.h>
00045 #include <pcl/kdtree/kdtree.h>
00046 #include <pcl/kdtree/kdtree_flann.h>
00047 #include <pcl/registration/transformation_validation.h>
00048
00049 namespace pcl
00050 {
00051 namespace registration
00052 {
00073 template <typename PointSource, typename PointTarget, typename Scalar = float>
00074 class TransformationValidationEuclidean
00075 {
00076 public:
00077 typedef typename TransformationValidation<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;
00078
00079 typedef boost::shared_ptr<TransformationValidation<PointSource, PointTarget, Scalar> > Ptr;
00080 typedef boost::shared_ptr<const TransformationValidation<PointSource, PointTarget, Scalar> > ConstPtr;
00081
00082 typedef typename pcl::search::KdTree<PointTarget> KdTree;
00083 typedef typename pcl::search::KdTree<PointTarget>::Ptr KdTreePtr;
00084
00085 typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr;
00086
00087 typedef typename TransformationValidation<PointSource, PointTarget>::PointCloudSourceConstPtr PointCloudSourceConstPtr;
00088 typedef typename TransformationValidation<PointSource, PointTarget>::PointCloudTargetConstPtr PointCloudTargetConstPtr;
00089
00094 TransformationValidationEuclidean () :
00095 max_range_ (std::numeric_limits<double>::max ()),
00096 threshold_ (std::numeric_limits<double>::quiet_NaN ()),
00097 tree_ (new pcl::search::KdTree<PointTarget>),
00098 force_no_recompute_ (false)
00099 {
00100 }
00101
00102 virtual ~TransformationValidationEuclidean () {};
00103
00108 inline void
00109 setMaxRange (double max_range)
00110 {
00111 max_range_ = max_range;
00112 }
00113
00117 inline double
00118 getMaxRange ()
00119 {
00120 return (max_range_);
00121 }
00122
00123
00131 inline void
00132 setSearchMethodTarget (const KdTreePtr &tree,
00133 bool force_no_recompute = false)
00134 {
00135 tree_ = tree;
00136 if (force_no_recompute)
00137 {
00138 force_no_recompute_ = true;
00139 }
00140 }
00141
00150 inline void
00151 setThreshold (double threshold)
00152 {
00153 threshold_ = threshold;
00154 }
00155
00157 inline double
00158 getThreshold ()
00159 {
00160 return (threshold_);
00161 }
00162
00172 double
00173 validateTransformation (
00174 const PointCloudSourceConstPtr &cloud_src,
00175 const PointCloudTargetConstPtr &cloud_tgt,
00176 const Matrix4 &transformation_matrix) const;
00177
00186 virtual bool
00187 operator() (const double &score1, const double &score2) const
00188 {
00189 return (score1 < score2);
00190 }
00191
00200 virtual bool
00201 isValid (
00202 const PointCloudSourceConstPtr &cloud_src,
00203 const PointCloudTargetConstPtr &cloud_tgt,
00204 const Matrix4 &transformation_matrix) const
00205 {
00206 if (pcl_isnan (threshold_))
00207 {
00208 PCL_ERROR ("[pcl::TransformationValidationEuclidean::isValid] Threshold not set! Please use setThreshold () before continuing.");
00209 return (false);
00210 }
00211
00212 return (validateTransformation (cloud_src, cloud_tgt, transformation_matrix) < threshold_);
00213 }
00214
00215 protected:
00219 double max_range_;
00220
00224 double threshold_;
00225
00227 KdTreePtr tree_;
00228
00231 bool force_no_recompute_;
00232
00233
00235 class MyPointRepresentation: public pcl::PointRepresentation<PointTarget>
00236 {
00237 using pcl::PointRepresentation<PointTarget>::nr_dimensions_;
00238 using pcl::PointRepresentation<PointTarget>::trivial_;
00239 public:
00240 typedef boost::shared_ptr<MyPointRepresentation> Ptr;
00241 typedef boost::shared_ptr<const MyPointRepresentation> ConstPtr;
00242
00243 MyPointRepresentation ()
00244 {
00245 nr_dimensions_ = 3;
00246 trivial_ = true;
00247 }
00248
00250 virtual ~MyPointRepresentation () {}
00251
00252 virtual void
00253 copyToFloatArray (const PointTarget &p, float * out) const
00254 {
00255 out[0] = p.x;
00256 out[1] = p.y;
00257 out[2] = p.z;
00258 }
00259 };
00260
00261 public:
00262 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00263 };
00264 }
00265 }
00266
00267 #include <pcl/registration/impl/transformation_validation_euclidean.hpp>
00268
00269 #endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_H_
00270