-
Notifications
You must be signed in to change notification settings - Fork 195
/
iterative_closest_point_2d.h
104 lines (81 loc) · 6.45 KB
/
iterative_closest_point_2d.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#pragma once
/**\file iterative_closest_point_2d.h
* \brief Description...
*
* @version 1.0
* @author Carlos Miguel Correia da Costa
*/
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <includes> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// std includes
#include <limits>
#include <memory>
#include <string>
// PCL includes
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_2D.h>
// project includes
#include <dynamic_robot_localization/cloud_matchers/point_matchers/iterative_closest_point.h>
#include <dynamic_robot_localization/convergence_estimators/default_convergence_criteria_with_time.h>
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> </includes> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
namespace dynamic_robot_localization {
// ############################################################### iterative_closest_point_time_constrained ################################################################
template <typename PointSource, typename PointTarget, typename Scalar = float>
class IterativeClosestPoint2DTimeConstrained: public pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar> {
public:
using Ptr = std::shared_ptr< IterativeClosestPoint2DTimeConstrained<PointSource, PointTarget, Scalar> >;
using ConstPtr = std::shared_ptr< const IterativeClosestPoint2DTimeConstrained<PointSource, PointTarget, Scalar> >;
IterativeClosestPoint2DTimeConstrained(double convergence_time_limit_seconds = std::numeric_limits<double>::max()) {
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::convergence_criteria_.reset(new DefaultConvergenceCriteriaWithTime<Scalar> (
pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_,
pcl::Registration<PointSource, PointTarget, Scalar>::transformation_,
*pcl::Registration<PointSource, PointTarget, Scalar>::correspondences_,
convergence_time_limit_seconds));
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_estimation_.reset(new pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>());
pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_ = "IterativeClosestPoint2D";
}
virtual ~IterativeClosestPoint2DTimeConstrained() {}
inline double getTransformCloudElapsedTime() { return transform_cloud_elapsed_time_ms_; }
inline void resetTransformCloudElapsedTime() { transform_cloud_elapsed_time_ms_ = 0; }
protected:
virtual void transformCloud(const typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource &input, typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource &output, const typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4 &transform) {
PerformanceTimer timer_;
timer_.start();
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud(input, output, transform);
transform_cloud_elapsed_time_ms_ += timer_.getElapsedTimeInMilliSec();
}
double transform_cloud_elapsed_time_ms_;
};
// ######################################################################## iterative_closest_point ########################################################################
/**
* \brief Description...
*/
template <typename PointT>
class IterativeClosestPoint2D : public IterativeClosestPoint<PointT> {
// ======================================================================== <public-section> ===========================================================================
public:
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <usings> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
using Ptr = std::shared_ptr< IterativeClosestPoint2D<PointT> >;
using ConstPtr = std::shared_ptr< const IterativeClosestPoint2D<PointT> >;
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> </usings> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <constructors-destructor> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
IterativeClosestPoint2D() {}
virtual ~IterativeClosestPoint2D() {}
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> </constructors-destructor> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <IterativeClosestPoint-functions> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
virtual void setupConfigurationFromParameterServer(ros::NodeHandlePtr& node_handle, ros::NodeHandlePtr& private_node_handle, const std::string& configuration_namespace);
virtual double getTransformCloudElapsedTimeMS();
virtual void resetTransformCloudElapsedTime();
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> </IterativeClosestPoint-functions> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <gets> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> </gets> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <sets> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> </sets> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// ======================================================================== </public-section> ===========================================================================
// ======================================================================== <protected-section> ========================================================================
protected:
// ======================================================================== </protected-section> ========================================================================
};
} /* namespace dynamic_robot_localization */
#ifdef DRL_NO_PRECOMPILE
#include <dynamic_robot_localization/cloud_matchers/point_matchers/impl/iterative_closest_point_2d.hpp>
#endif