69 lines
2.6 KiB
C
69 lines
2.6 KiB
C
|
// ----------------------------------------------------------------------------
|
||
|
|
// - Open3D: www.open3d.org -
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Copyright (c) 2018-2023 www.open3d.org
|
||
|
|
// SPDX-License-Identifier: MIT
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <Eigen/Core>
|
||
|
|
#include <iostream>
|
||
|
|
#include <tuple>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#include "open3d/camera/PinholeCameraIntrinsic.h"
|
||
|
|
#include "open3d/pipelines/odometry/OdometryOption.h"
|
||
|
|
#include "open3d/pipelines/odometry/RGBDOdometryJacobian.h"
|
||
|
|
#include "open3d/utility/Eigen.h"
|
||
|
|
#include "open3d/utility/Logging.h"
|
||
|
|
|
||
|
|
namespace open3d {
|
||
|
|
|
||
|
|
namespace geometry {
|
||
|
|
class RGBDImage;
|
||
|
|
}
|
||
|
|
|
||
|
|
namespace pipelines {
|
||
|
|
namespace odometry {
|
||
|
|
|
||
|
|
/// \brief Function to estimate 6D rigid motion from two RGBD image pairs.
|
||
|
|
///
|
||
|
|
/// \param source Source RGBD image.
|
||
|
|
/// \param target Target RGBD image.
|
||
|
|
/// \param pinhole_camera_intrinsic Camera intrinsic parameters.
|
||
|
|
/// \param odo_init Initial 4x4 motion matrix estimation.
|
||
|
|
/// \param jacobian_method The odometry Jacobian method to use.
|
||
|
|
/// \param option Odometry hyper parameters.
|
||
|
|
/// \return is_success, 4x4 motion matrix, 6x6 information matrix.
|
||
|
|
std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> ComputeRGBDOdometry(
|
||
|
|
const geometry::RGBDImage &source,
|
||
|
|
const geometry::RGBDImage &target,
|
||
|
|
const camera::PinholeCameraIntrinsic &pinhole_camera_intrinsic =
|
||
|
|
camera::PinholeCameraIntrinsic(),
|
||
|
|
const Eigen::Matrix4d &odo_init = Eigen::Matrix4d::Identity(),
|
||
|
|
const RGBDOdometryJacobian &jacobian_method =
|
||
|
|
RGBDOdometryJacobianFromHybridTerm(),
|
||
|
|
const OdometryOption &option = OdometryOption());
|
||
|
|
|
||
|
|
/// \brief Function to estimate point to point correspondences from two depth
|
||
|
|
/// images.
|
||
|
|
///
|
||
|
|
/// \param intrinsic_matrix Camera intrinsic parameters.
|
||
|
|
/// \param extrinsic Estimation of transform from source to target.
|
||
|
|
/// \param depth_s Source depth image.
|
||
|
|
/// \param depth_t Target depth image.
|
||
|
|
/// \param option Odometry hyper parameters.
|
||
|
|
/// \return A vector of u_s, v_s, u_t, v_t which maps the 2d coordinates of
|
||
|
|
/// source to target.
|
||
|
|
CorrespondenceSetPixelWise ComputeCorrespondence(
|
||
|
|
const Eigen::Matrix3d &intrinsic_matrix,
|
||
|
|
const Eigen::Matrix4d &extrinsic,
|
||
|
|
const geometry::Image &depth_s,
|
||
|
|
const geometry::Image &depth_t,
|
||
|
|
const OdometryOption &option);
|
||
|
|
|
||
|
|
} // namespace odometry
|
||
|
|
} // namespace pipelines
|
||
|
|
} // namespace open3d
|