Program Listing for File base-alignment.h

Return to documentation for file (algorithms/registration-toolbox/include/registration-toolbox/alignment/base-alignment.h)

#ifndef REGISTRATION_TOOLBOX_ALIGNMENT_BASE_ALIGNMENT_H_
#define REGISTRATION_TOOLBOX_ALIGNMENT_BASE_ALIGNMENT_H_

#include <memory>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "registration-toolbox/model/registration-result.h"

namespace regbox {

template <typename T_depth_input>
class BaseAlignment {
 public:
  virtual ~BaseAlignment() = default;
  RegistrationResult registerCloud(
      const T_depth_input& target, const T_depth_input& source,
      const aslam::Transformation& prior_T_target_source);

 protected:
  virtual RegistrationResult registerCloudImpl(
      const T_depth_input& target, const T_depth_input& source,
      const aslam::Transformation& prior_T_target_source) = 0;

  constexpr aslam::Transformation convertEigenToKindr(
      const Eigen::Matrix4d& T) const {
    if (!aslam::Transformation::Rotation::isValidRotationMatrix(
            T.template topLeftCorner<3, 3>().eval())) {
      return aslam::Transformation::constructAndRenormalizeRotation(T);
    }
    return aslam::Transformation(T);
  }
};

template <typename T_depth_input>
RegistrationResult BaseAlignment<T_depth_input>::registerCloud(
    const T_depth_input& target, const T_depth_input& source,
    const aslam::Transformation& prior_T_target_source) {
  CHECK_NOTNULL(target);
  CHECK_NOTNULL(source);
  return registerCloudImpl(target, source, prior_T_target_source);
}

}  // namespace regbox

#endif  // REGISTRATION_TOOLBOX_ALIGNMENT_BASE_ALIGNMENT_H_