Skip to content

Commit ebc0a6a

Browse files
keineahnung2345keineahnung2345
authored
[registration] add Scalar template variable to NormalDistributionsTransform (#5291)
* [feat] add Scalar template variable to NormalDistributionsTransform * [fix] make Vector3 typename * [fix] error: expected primary-expression before 'double' * [refactor] formatting
1 parent d381bb8 commit ebc0a6a

2 files changed

Lines changed: 69 additions & 62 deletions

File tree

‎registration/include/pcl/registration/impl/ndt.hpp‎

Lines changed: 30 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -43,8 +43,9 @@
4343

4444
namespace pcl {
4545

46-
template <typename PointSource, typename PointTarget>
47-
NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform()
46+
template <typename PointSource, typename PointTarget, typename Scalar>
47+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::
48+
NormalDistributionsTransform()
4849
: target_cells_()
4950
, resolution_(1.0f)
5051
, step_size_(0.1)
@@ -68,10 +69,10 @@ NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTrans
6869
max_iterations_ = 35;
6970
}
7071

71-
template <typename PointSource, typename PointTarget>
72+
template <typename PointSource, typename PointTarget, typename Scalar>
7273
void
73-
NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation(
74-
PointCloudSource& output, const Eigen::Matrix4f& guess)
74+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::computeTransformation(
75+
PointCloudSource& output, const Matrix4& guess)
7576
{
7677
nr_iterations_ = 0;
7778
converged_ = false;
@@ -85,7 +86,7 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation(
8586
-2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
8687
gauss_d1_);
8788

88-
if (guess != Eigen::Matrix4f::Identity()) {
89+
if (guess != Matrix4::Identity()) {
8990
// Initialise final transformation to the guessed one
9091
final_transformation_ = guess;
9192
// Apply guessed transformation prior to search for neighbours
@@ -97,14 +98,15 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation(
9798
point_jacobian_.block<3, 3>(0, 0).setIdentity();
9899
point_hessian_.setZero();
99100

100-
Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
101+
Eigen::Transform<Scalar, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
101102
eig_transformation.matrix() = final_transformation_;
102103

103104
// Convert initial guess matrix to 6 element transformation vector
104105
Eigen::Matrix<double, 6, 1> transform, score_gradient;
105-
Eigen::Vector3f init_translation = eig_transformation.translation();
106-
Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);
107-
transform << init_translation.cast<double>(), init_rotation.cast<double>();
106+
Vector3 init_translation = eig_transformation.translation();
107+
Vector3 init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);
108+
transform << init_translation.template cast<double>(),
109+
init_rotation.template cast<double>();
108110

109111
Eigen::Matrix<double, 6, 6> hessian;
110112

@@ -179,9 +181,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation(
179181
trans_likelihood_ = score / static_cast<double>(input_->size());
180182
}
181183

182-
template <typename PointSource, typename PointTarget>
184+
template <typename PointSource, typename PointTarget, typename Scalar>
183185
double
184-
NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives(
186+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::computeDerivatives(
185187
Eigen::Matrix<double, 6, 1>& score_gradient,
186188
Eigen::Matrix<double, 6, 6>& hessian,
187189
const PointCloudSource& trans_cloud,
@@ -230,9 +232,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives(
230232
return score;
231233
}
232234

233-
template <typename PointSource, typename PointTarget>
235+
template <typename PointSource, typename PointTarget, typename Scalar>
234236
void
235-
NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives(
237+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::computeAngleDerivatives(
236238
const Eigen::Matrix<double, 6, 1>& transform, bool compute_hessian)
237239
{
238240
// Simplified math for near 0 angles
@@ -315,9 +317,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives(
315317
}
316318
}
317319

318-
template <typename PointSource, typename PointTarget>
320+
template <typename PointSource, typename PointTarget, typename Scalar>
319321
void
320-
NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(
322+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::computePointDerivatives(
321323
const Eigen::Vector3d& x, bool compute_hessian)
322324
{
323325
// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector.
@@ -361,9 +363,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(
361363
}
362364
}
363365

364-
template <typename PointSource, typename PointTarget>
366+
template <typename PointSource, typename PointTarget, typename Scalar>
365367
double
366-
NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives(
368+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::updateDerivatives(
367369
Eigen::Matrix<double, 6, 1>& score_gradient,
368370
Eigen::Matrix<double, 6, 6>& hessian,
369371
const Eigen::Vector3d& x_trans,
@@ -409,9 +411,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives(
409411
return score_inc;
410412
}
411413

412-
template <typename PointSource, typename PointTarget>
414+
template <typename PointSource, typename PointTarget, typename Scalar>
413415
void
414-
NormalDistributionsTransform<PointSource, PointTarget>::computeHessian(
416+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::computeHessian(
415417
Eigen::Matrix<double, 6, 6>& hessian, const PointCloudSource& trans_cloud)
416418
{
417419
hessian.setZero();
@@ -451,9 +453,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeHessian(
451453
}
452454
}
453455

454-
template <typename PointSource, typename PointTarget>
456+
template <typename PointSource, typename PointTarget, typename Scalar>
455457
void
456-
NormalDistributionsTransform<PointSource, PointTarget>::updateHessian(
458+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::updateHessian(
457459
Eigen::Matrix<double, 6, 6>& hessian,
458460
const Eigen::Vector3d& x_trans,
459461
const Eigen::Matrix3d& c_inv) const
@@ -486,9 +488,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::updateHessian(
486488
}
487489
}
488490

489-
template <typename PointSource, typename PointTarget>
491+
template <typename PointSource, typename PointTarget, typename Scalar>
490492
bool
491-
NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT(
493+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::updateIntervalMT(
492494
double& a_l,
493495
double& f_l,
494496
double& g_l,
@@ -531,9 +533,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT(
531533
return true;
532534
}
533535

534-
template <typename PointSource, typename PointTarget>
536+
template <typename PointSource, typename PointTarget, typename Scalar>
535537
double
536-
NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT(
538+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::trialValueSelectionMT(
537539
double a_l,
538540
double f_l,
539541
double g_l,
@@ -644,9 +646,9 @@ NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT(
644646
}
645647
}
646648

647-
template <typename PointSource, typename PointTarget>
649+
template <typename PointSource, typename PointTarget, typename Scalar>
648650
double
649-
NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT(
651+
NormalDistributionsTransform<PointSource, PointTarget, Scalar>::computeStepLengthMT(
650652
const Eigen::Matrix<double, 6, 1>& x,
651653
Eigen::Matrix<double, 6, 1>& step_dir,
652654
double step_init,

‎registration/include/pcl/registration/ndt.h‎

Lines changed: 39 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -61,16 +61,17 @@ namespace pcl {
6161
* \note Math refactored by Todor Stoyanov.
6262
* \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
6363
*/
64-
template <typename PointSource, typename PointTarget>
65-
class NormalDistributionsTransform : public Registration<PointSource, PointTarget> {
64+
template <typename PointSource, typename PointTarget, typename Scalar = float>
65+
class NormalDistributionsTransform
66+
: public Registration<PointSource, PointTarget, Scalar> {
6667
protected:
6768
using PointCloudSource =
68-
typename Registration<PointSource, PointTarget>::PointCloudSource;
69+
typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
6970
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
7071
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
7172

7273
using PointCloudTarget =
73-
typename Registration<PointSource, PointTarget>::PointCloudTarget;
74+
typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
7475
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
7576
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
7677

@@ -88,9 +89,13 @@ class NormalDistributionsTransform : public Registration<PointSource, PointTarge
8889
using TargetGridLeafConstPtr = typename TargetGrid::LeafConstPtr;
8990

9091
public:
91-
using Ptr = shared_ptr<NormalDistributionsTransform<PointSource, PointTarget>>;
92+
using Ptr =
93+
shared_ptr<NormalDistributionsTransform<PointSource, PointTarget, Scalar>>;
9294
using ConstPtr =
93-
shared_ptr<const NormalDistributionsTransform<PointSource, PointTarget>>;
95+
shared_ptr<const NormalDistributionsTransform<PointSource, PointTarget, Scalar>>;
96+
using Vector3 = typename Eigen::Matrix<Scalar, 3, 1>;
97+
using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
98+
using Affine3 = typename Eigen::Transform<Scalar, 3, Eigen::Affine>;
9499

95100
/** \brief Constructor. Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to
96101
* 0.05 and \ref resolution_ to 1.0
@@ -107,7 +112,7 @@ class NormalDistributionsTransform : public Registration<PointSource, PointTarge
107112
inline void
108113
setInputTarget(const PointCloudTargetConstPtr& cloud) override
109114
{
110-
Registration<PointSource, PointTarget>::setInputTarget(cloud);
115+
Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
111116
init();
112117
}
113118

@@ -208,12 +213,12 @@ class NormalDistributionsTransform : public Registration<PointSource, PointTarge
208213
* vector
209214
*/
210215
static void
211-
convertTransform(const Eigen::Matrix<double, 6, 1>& x, Eigen::Affine3f& trans)
216+
convertTransform(const Eigen::Matrix<double, 6, 1>& x, Affine3& trans)
212217
{
213-
trans = Eigen::Translation<float, 3>(x.head<3>().cast<float>()) *
214-
Eigen::AngleAxis<float>(float(x(3)), Eigen::Vector3f::UnitX()) *
215-
Eigen::AngleAxis<float>(float(x(4)), Eigen::Vector3f::UnitY()) *
216-
Eigen::AngleAxis<float>(float(x(5)), Eigen::Vector3f::UnitZ());
218+
trans = Eigen::Translation<Scalar, 3>(x.head<3>().cast<Scalar>()) *
219+
Eigen::AngleAxis<Scalar>(Scalar(x(3)), Vector3::UnitX()) *
220+
Eigen::AngleAxis<Scalar>(Scalar(x(4)), Vector3::UnitY()) *
221+
Eigen::AngleAxis<Scalar>(Scalar(x(5)), Vector3::UnitZ());
217222
}
218223

219224
/** \brief Convert 6 element transformation vector to transformation matrix.
@@ -222,31 +227,32 @@ class NormalDistributionsTransform : public Registration<PointSource, PointTarge
222227
* transformation vector
223228
*/
224229
static void
225-
convertTransform(const Eigen::Matrix<double, 6, 1>& x, Eigen::Matrix4f& trans)
230+
convertTransform(const Eigen::Matrix<double, 6, 1>& x, Matrix4& trans)
226231
{
227-
Eigen::Affine3f _affine;
232+
Affine3 _affine;
228233
convertTransform(x, _affine);
229234
trans = _affine.matrix();
230235
}
231236

232237
protected:
233-
using Registration<PointSource, PointTarget>::reg_name_;
234-
using Registration<PointSource, PointTarget>::getClassName;
235-
using Registration<PointSource, PointTarget>::input_;
236-
using Registration<PointSource, PointTarget>::indices_;
237-
using Registration<PointSource, PointTarget>::target_;
238-
using Registration<PointSource, PointTarget>::nr_iterations_;
239-
using Registration<PointSource, PointTarget>::max_iterations_;
240-
using Registration<PointSource, PointTarget>::previous_transformation_;
241-
using Registration<PointSource, PointTarget>::final_transformation_;
242-
using Registration<PointSource, PointTarget>::transformation_;
243-
using Registration<PointSource, PointTarget>::transformation_epsilon_;
244-
using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
245-
using Registration<PointSource, PointTarget>::converged_;
246-
using Registration<PointSource, PointTarget>::corr_dist_threshold_;
247-
using Registration<PointSource, PointTarget>::inlier_threshold_;
248-
249-
using Registration<PointSource, PointTarget>::update_visualizer_;
238+
using Registration<PointSource, PointTarget, Scalar>::reg_name_;
239+
using Registration<PointSource, PointTarget, Scalar>::getClassName;
240+
using Registration<PointSource, PointTarget, Scalar>::input_;
241+
using Registration<PointSource, PointTarget, Scalar>::indices_;
242+
using Registration<PointSource, PointTarget, Scalar>::target_;
243+
using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
244+
using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
245+
using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
246+
using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
247+
using Registration<PointSource, PointTarget, Scalar>::transformation_;
248+
using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
249+
using Registration<PointSource, PointTarget, Scalar>::
250+
transformation_rotation_epsilon_;
251+
using Registration<PointSource, PointTarget, Scalar>::converged_;
252+
using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
253+
using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
254+
255+
using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
250256

251257
/** \brief Estimate the transformation and returns the transformed source
252258
* (input) as output.
@@ -255,7 +261,7 @@ class NormalDistributionsTransform : public Registration<PointSource, PointTarge
255261
virtual void
256262
computeTransformation(PointCloudSource& output)
257263
{
258-
computeTransformation(output, Eigen::Matrix4f::Identity());
264+
computeTransformation(output, Matrix4::Identity());
259265
}
260266

261267
/** \brief Estimate the transformation and returns the transformed source
@@ -264,8 +270,7 @@ class NormalDistributionsTransform : public Registration<PointSource, PointTarge
264270
* \param[in] guess the initial gross estimation of the transformation
265271
*/
266272
void
267-
computeTransformation(PointCloudSource& output,
268-
const Eigen::Matrix4f& guess) override;
273+
computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
269274

270275
/** \brief Initiate covariance voxel structure. */
271276
void inline init()

0 commit comments

Comments
 (0)