18 #ifndef POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_H_
19 #define POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_H_
26 #include <ceres/ceres.h>
27 #include <ceres/solver.h>
29 #include <boost/optional.hpp>
38 boost::optional<localization_common::PoseWithCovariance>
Align(
39 const std::vector<Eigen::Vector3d>& source_points,
const std::vector<Eigen::Vector3d>& target_points,
41 const boost::optional<
const std::vector<Eigen::Vector3d>&> source_normals = boost::none,
42 const boost::optional<
const std::vector<Eigen::Vector3d>&> target_normals = boost::none)
const;
45 const std::vector<Eigen::Vector3d>& source_points,
const std::vector<Eigen::Vector3d>& target_points,
46 const boost::optional<
const std::vector<Eigen::Vector3d>&> source_normals = boost::none,
47 const boost::optional<
const std::vector<Eigen::Vector3d>&> target_normals = boost::none,
48 const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity())
const;
51 const std::vector<Eigen::Vector3d>& source_points,
const std::vector<Eigen::Vector3d>& target_points,
52 const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity())
const;
55 boost::optional<localization_common::PoseCovariance> Covariance(
const double*
const target_T_source_data,
56 ceres::Problem& problem)
const;
62 #endif // POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_H_