v01
This commit is contained in:
Ivan
303
thirdparty/basalt-headers/include/basalt/camera/bal_camera.hpp
vendored
Normal file
303
thirdparty/basalt-headers/include/basalt/camera/bal_camera.hpp
vendored
Normal file
@@ -0,0 +1,303 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of unified camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/camera_static_assert.hpp>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Camera model used in the paper "Bundle Adjustment in the Large".
|
||||
///
|
||||
/// See https://grail.cs.washington.edu/projects/bal/ for details.
|
||||
/// This model has N=3 parameters \f$ \mathbf{i} = \left[f, k_1, k_2
|
||||
/// \right]^T \f$.
|
||||
///
|
||||
/// Unlike the original formulation we assume that the POSITIVE z-axis
|
||||
/// points in camera direction and thus don't include the "minus" in the
|
||||
/// perspective projection. You need to consider this when loading BAL data.
|
||||
///
|
||||
/// Specifically, for the camera frame we assume the positive z axis pointing
|
||||
/// forward in view direction and in the image, y is poiting down, x to the
|
||||
/// right. In the original BAL formulation, the camera points in negative z
|
||||
/// axis, y is up in the image. Thus when loading the data, we invert the y and
|
||||
/// z camera axes (y also in the image) in the perspective projection, we don't
|
||||
/// have the "minus" like in the original Snavely model.
|
||||
///
|
||||
/// A 3D point P in camera coordinates is mapped to pixel coordinates p':
|
||||
/// p = [P / P.z]_xy (perspective division)
|
||||
/// p' = f * r(p) * p (conversion to pixel coordinates)
|
||||
/// r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4.
|
||||
///
|
||||
/// See \ref project and \ref unproject functions for more details.
|
||||
template <typename Scalar_ = double>
|
||||
class BalCamera {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int N = 3; ///< Number of intrinsic parameters.
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using VecN = Eigen::Matrix<Scalar, N, 1>;
|
||||
|
||||
using Mat2 = Eigen::Matrix<Scalar, 2, 2>;
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat2N = Eigen::Matrix<Scalar, 2, N>;
|
||||
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4N = Eigen::Matrix<Scalar, 4, N>;
|
||||
|
||||
/// @brief Default constructor with zero intrinsics
|
||||
BalCamera() { param_.setZero(); }
|
||||
|
||||
/// @brief Construct camera model with given vector of intrinsics
|
||||
///
|
||||
/// @param[in] p vector of intrinsic parameters [f, k1, k2]
|
||||
explicit BalCamera(const VecN& p) { param_ = p; }
|
||||
|
||||
/// @brief Cast to different scalar type
|
||||
template <class Scalar2>
|
||||
BalCamera<Scalar2> cast() const {
|
||||
return BalCamera<Scalar2>(param_.template cast<Scalar2>());
|
||||
}
|
||||
|
||||
/// @brief Camera model name
|
||||
///
|
||||
/// @return "bal"
|
||||
static std::string getName() { return "bal"; }
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobians
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @param[out] d_proj_d_param point if not nullptr computed Jacobian of
|
||||
/// projection with respect to intrinsic parameters
|
||||
/// @return if projection is valid
|
||||
template <class DerivedPoint3D, class DerivedPoint2D,
|
||||
class DerivedJ3D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool project(const Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
DerivedJ3D d_proj_d_p3d = nullptr,
|
||||
DerivedJparam d_proj_d_param = nullptr) const {
|
||||
checkProjectionDerivedTypes<DerivedPoint3D, DerivedPoint2D, DerivedJ3D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint3D>::Type p3d_eval(p3d);
|
||||
|
||||
const Scalar& f = param_[0];
|
||||
const Scalar& k1 = param_[1];
|
||||
const Scalar& k2 = param_[2];
|
||||
|
||||
const Scalar& x = p3d_eval[0];
|
||||
const Scalar& y = p3d_eval[1];
|
||||
const Scalar& z = p3d_eval[2];
|
||||
|
||||
const Scalar mx = x / z;
|
||||
const Scalar my = y / z;
|
||||
|
||||
const Scalar mx2 = mx * mx;
|
||||
const Scalar my2 = my * my;
|
||||
|
||||
const Scalar r2 = mx2 + my2;
|
||||
const Scalar r4 = r2 * r2;
|
||||
|
||||
const Scalar rp = Scalar(1) + k1 * r2 + k2 * r4;
|
||||
|
||||
proj = Vec2(f * mx * rp, f * my * rp);
|
||||
|
||||
const bool is_valid = z >= Sophus::Constants<Scalar>::epsilonSqrt();
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
d_proj_d_p3d->setZero();
|
||||
|
||||
const Scalar tmp = k1 + k2 * Scalar(2) * r2;
|
||||
|
||||
(*d_proj_d_p3d)(0, 0) = f * (rp + Scalar(2) * mx2 * tmp) / z;
|
||||
(*d_proj_d_p3d)(1, 1) = f * (rp + Scalar(2) * my2 * tmp) / z;
|
||||
|
||||
(*d_proj_d_p3d)(1, 0) = (*d_proj_d_p3d)(0, 1) =
|
||||
f * my * mx * Scalar(2) * tmp / z;
|
||||
|
||||
(*d_proj_d_p3d)(0, 2) = -f * mx * (rp + Scalar(2) * tmp * r2) / z;
|
||||
(*d_proj_d_p3d)(1, 2) = -f * my * (rp + Scalar(2) * tmp * r2) / z;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
(*d_proj_d_param).setZero();
|
||||
(*d_proj_d_param)(0, 0) = mx * rp;
|
||||
(*d_proj_d_param)(1, 0) = my * rp;
|
||||
(*d_proj_d_param)(0, 1) = f * mx * r2;
|
||||
(*d_proj_d_param)(1, 1) = f * my * r2;
|
||||
(*d_proj_d_param)(0, 2) = f * mx * r4;
|
||||
(*d_proj_d_param)(1, 2) = f * my * r4;
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobians
|
||||
///
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @param[out] d_p3d_d_param point if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to intrinsic parameters
|
||||
/// @return if unprojection is valid
|
||||
template <class DerivedPoint2D, class DerivedPoint3D,
|
||||
class DerivedJ2D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool unproject(const Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
DerivedJ2D d_p3d_d_proj = nullptr,
|
||||
DerivedJparam d_p3d_d_param = nullptr) const {
|
||||
checkUnprojectionDerivedTypes<DerivedPoint2D, DerivedPoint3D, DerivedJ2D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint2D>::Type proj_eval(proj);
|
||||
|
||||
const Scalar& f = param_[0];
|
||||
const Scalar& k1 = param_[1];
|
||||
const Scalar& k2 = param_[2];
|
||||
|
||||
const Scalar& u = proj_eval[0];
|
||||
const Scalar& v = proj_eval[1];
|
||||
|
||||
const Vec2 pp(u / f, v / f);
|
||||
|
||||
Vec2 p = pp;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
const Scalar r2 = p.squaredNorm();
|
||||
|
||||
const Scalar rp = (Scalar(1) + k1 * r2 + k2 * r2 * r2);
|
||||
const Vec2 pp_computed = p * rp;
|
||||
|
||||
const Scalar tmp = k1 + k2 * Scalar(2) * r2;
|
||||
|
||||
Mat2 J_p;
|
||||
J_p(0, 0) = (rp + Scalar(2) * p[0] * p[0] * tmp);
|
||||
J_p(1, 1) = (rp + Scalar(2) * p[1] * p[1] * tmp);
|
||||
J_p(1, 0) = J_p(0, 1) = p[0] * p[1] * Scalar(2) * tmp;
|
||||
|
||||
const Vec2 dp = (J_p.transpose() * J_p).inverse() * J_p.transpose() *
|
||||
(pp_computed - pp);
|
||||
|
||||
p -= dp;
|
||||
}
|
||||
|
||||
p3d.setZero();
|
||||
p3d[0] = p[0];
|
||||
p3d[1] = p[1];
|
||||
p3d[2] = 1;
|
||||
|
||||
p3d.normalize();
|
||||
|
||||
BASALT_ASSERT_STREAM(d_p3d_d_proj == nullptr && d_p3d_d_param == nullptr,
|
||||
"Jacobians for unprojection are not implemented");
|
||||
UNUSED(d_p3d_d_proj);
|
||||
UNUSED(d_p3d_d_param);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// Initializes the camera model to \f$ \left[f_x, 0, 0 \right]^T \f$
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
param_[0] = init[0];
|
||||
param_[1] = 0;
|
||||
param_[2] = 0;
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc and clamp the values to the
|
||||
/// valid range
|
||||
///
|
||||
/// @param[in] inc increment vector
|
||||
void operator+=(const VecN& inc) { param_ += inc; }
|
||||
|
||||
/// @brief Returns a const reference to the intrinsic parameters vector
|
||||
///
|
||||
/// The order is following: \f$ \left[f, k1, k2 \right]^T \f$
|
||||
/// @return const reference to the intrinsic parameters vector
|
||||
const VecN& getParam() const { return param_; }
|
||||
|
||||
/// @brief Projections used for unit-tests
|
||||
static Eigen::aligned_vector<BalCamera> getTestProjections() {
|
||||
Eigen::aligned_vector<BalCamera> res;
|
||||
|
||||
VecN vec1;
|
||||
|
||||
vec1 << 399.752, -3.78048e-05, 5.37738e-07;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Resolutions used for unit-tests
|
||||
static Eigen::aligned_vector<Eigen::Vector2i> getTestResolutions() {
|
||||
Eigen::aligned_vector<Eigen::Vector2i> res;
|
||||
|
||||
res.emplace_back(640, 480);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
VecN param_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
126
thirdparty/basalt-headers/include/basalt/camera/camera_static_assert.hpp
vendored
Normal file
126
thirdparty/basalt-headers/include/basalt/camera/camera_static_assert.hpp
vendored
Normal file
@@ -0,0 +1,126 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Static asserts for checking projection and unprojection types
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <string_view>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
/// @brief Helper struct to evaluate lazy Eigen expressions or const reference
|
||||
/// them if they are Eigen::Matrix or Eigen::Map types
|
||||
template <class Derived>
|
||||
struct EvalOrReference {
|
||||
using Type = typename Eigen::internal::eval<Derived>::type;
|
||||
};
|
||||
|
||||
/// @brief Helper struct to evaluate lazy Eigen expressions or const reference
|
||||
/// them if they are Eigen::Matrix or Eigen::Map types
|
||||
template <typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows,
|
||||
int _MaxCols, int MapOptions, typename StrideType>
|
||||
struct EvalOrReference<Eigen::Map<
|
||||
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>,
|
||||
MapOptions, StrideType>> {
|
||||
using Type = const Eigen::MatrixBase<Eigen::Map<
|
||||
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>,
|
||||
MapOptions, StrideType>> &;
|
||||
};
|
||||
|
||||
/// @brief Helper function to check that 3D points are 3 or 4 dimensional and
|
||||
/// the Jacobians have the appropriate shape in the project function
|
||||
template <class DerivedPoint3D, class DerivedPoint2D, class DerivedJ3DPtr,
|
||||
class DerivedJparamPtr, int N>
|
||||
constexpr inline void checkProjectionDerivedTypes() {
|
||||
EIGEN_STATIC_ASSERT(DerivedPoint3D::IsVectorAtCompileTime &&
|
||||
(DerivedPoint3D::SizeAtCompileTime == 3 ||
|
||||
DerivedPoint3D::SizeAtCompileTime == 4),
|
||||
THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE)
|
||||
|
||||
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedPoint2D, 2);
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3DPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJ3DPtr>);
|
||||
using DerivedJ3D = typename std::remove_pointer<DerivedJ3DPtr>::type;
|
||||
|
||||
EIGEN_STATIC_ASSERT(DerivedJ3D::RowsAtCompileTime == 2 &&
|
||||
int(DerivedJ3D::ColsAtCompileTime) ==
|
||||
int(DerivedPoint3D::SizeAtCompileTime),
|
||||
THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparamPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJparamPtr>);
|
||||
using DerivedJparam = typename std::remove_pointer<DerivedJparamPtr>::type;
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedJparam, 2, N);
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Helper function to check that 3D points are 3 or 4 dimensional and
|
||||
/// the Jacobians have the appropriate shape in the unproject function
|
||||
template <class DerivedPoint2D, class DerivedPoint3D, class DerivedJ2DPtr,
|
||||
class DerivedJparamPtr, int N>
|
||||
constexpr inline void checkUnprojectionDerivedTypes() {
|
||||
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedPoint2D, 2);
|
||||
|
||||
EIGEN_STATIC_ASSERT(DerivedPoint3D::IsVectorAtCompileTime &&
|
||||
(DerivedPoint3D::SizeAtCompileTime == 3 ||
|
||||
DerivedPoint3D::SizeAtCompileTime == 4),
|
||||
THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE)
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2DPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJ2DPtr>);
|
||||
using DerivedJ2D = typename std::remove_pointer<DerivedJ2DPtr>::type;
|
||||
EIGEN_STATIC_ASSERT(DerivedJ2D::ColsAtCompileTime == 2 &&
|
||||
int(DerivedJ2D::RowsAtCompileTime) ==
|
||||
int(DerivedPoint3D::SizeAtCompileTime),
|
||||
THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparamPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJparamPtr>);
|
||||
using DerivedJparam = typename std::remove_pointer<DerivedJparamPtr>::type;
|
||||
EIGEN_STATIC_ASSERT(DerivedJparam::ColsAtCompileTime == N &&
|
||||
int(DerivedJparam::RowsAtCompileTime) ==
|
||||
int(DerivedPoint3D::SizeAtCompileTime),
|
||||
THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace basalt
|
||||
448
thirdparty/basalt-headers/include/basalt/camera/double_sphere_camera.hpp
vendored
Normal file
448
thirdparty/basalt-headers/include/basalt/camera/double_sphere_camera.hpp
vendored
Normal file
@@ -0,0 +1,448 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of double sphere camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/camera_static_assert.hpp>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Double Sphere camera model
|
||||
///
|
||||
/// \image html ds.png
|
||||
/// This model has N=6 parameters \f$ \mathbf{i} = \left[f_x, f_y, c_x, c_y,
|
||||
/// \xi, \alpha \right]^T \f$ with \f$ \xi \in [-1,1], \alpha \in [0,1] \f$. See
|
||||
/// \ref project and \ref unproject functions for more details.
|
||||
template <typename Scalar_ = double>
|
||||
class DoubleSphereCamera {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int N = 6; ///< Number of intrinsic parameters.
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using VecN = Eigen::Matrix<Scalar, N, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat2N = Eigen::Matrix<Scalar, 2, N>;
|
||||
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4N = Eigen::Matrix<Scalar, 4, N>;
|
||||
|
||||
/// @brief Default constructor with zero intrinsics
|
||||
DoubleSphereCamera() { param_.setZero(); }
|
||||
|
||||
/// @brief Construct camera model with given vector of intrinsics
|
||||
///
|
||||
/// @param[in] p vector of intrinsic parameters [fx, fy, cx, cy, xi, alpha]
|
||||
explicit DoubleSphereCamera(const VecN& p) { param_ = p; }
|
||||
|
||||
/// @brief Cast to different scalar type
|
||||
template <class Scalar2>
|
||||
DoubleSphereCamera<Scalar2> cast() const {
|
||||
return DoubleSphereCamera<Scalar2>(param_.template cast<Scalar2>());
|
||||
}
|
||||
|
||||
/// @brief Camera model name
|
||||
///
|
||||
/// @return "ds"
|
||||
static std::string getName() { return "ds"; }
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobians
|
||||
///
|
||||
/// Projection function is defined as follows:
|
||||
/// \f{align}{
|
||||
/// \pi(\mathbf{x}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// f_x{\frac{x}{\alpha d_2 + (1-\alpha) (\xi d_1 + z)}}
|
||||
/// \\ f_y{\frac{y}{\alpha d_2 + (1-\alpha) (\xi d_1 + z)}}
|
||||
/// \\ \end{bmatrix}
|
||||
/// +
|
||||
/// \begin{bmatrix}
|
||||
/// c_x
|
||||
/// \\ c_y
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ d_1 &= \sqrt{x^2 + y^2 + z^2},
|
||||
/// \\ d_2 &= \sqrt{x^2 + y^2 + (\xi d_1 + z)^2}.
|
||||
/// \f}
|
||||
/// A set of 3D points that results in valid projection is expressed as
|
||||
/// follows: \f{align}{
|
||||
/// \Omega &= \{\mathbf{x} \in \mathbb{R}^3 ~|~ z > -w_2 d_1 \}
|
||||
/// \\ w_2 &= \frac{w_1+\xi}{\sqrt{2w_1\xi + \xi^2 + 1}}
|
||||
/// \\ w_1 &= \begin{cases} \frac{\alpha}{1-\alpha}, & \mbox{if } \alpha
|
||||
/// \le 0.5 \\ \frac{1-\alpha}{\alpha} & \mbox{if } \alpha > 0.5
|
||||
/// \end{cases}
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @param[out] d_proj_d_param point if not nullptr computed Jacobian of
|
||||
/// projection with respect to intrinsic parameters
|
||||
/// @return if projection is valid
|
||||
template <class DerivedPoint3D, class DerivedPoint2D,
|
||||
class DerivedJ3D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool project(const Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
DerivedJ3D d_proj_d_p3d = nullptr,
|
||||
DerivedJparam d_proj_d_param = nullptr) const {
|
||||
checkProjectionDerivedTypes<DerivedPoint3D, DerivedPoint2D, DerivedJ3D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint3D>::Type p3d_eval(p3d);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
|
||||
const Scalar& xi = param_[4];
|
||||
const Scalar& alpha = param_[5];
|
||||
|
||||
const Scalar& x = p3d_eval[0];
|
||||
const Scalar& y = p3d_eval[1];
|
||||
const Scalar& z = p3d_eval[2];
|
||||
|
||||
const Scalar xx = x * x;
|
||||
const Scalar yy = y * y;
|
||||
const Scalar zz = z * z;
|
||||
|
||||
const Scalar r2 = xx + yy;
|
||||
|
||||
const Scalar d1_2 = r2 + zz;
|
||||
const Scalar d1 = sqrt(d1_2);
|
||||
|
||||
const Scalar w1 = alpha > Scalar(0.5) ? (Scalar(1) - alpha) / alpha
|
||||
: alpha / (Scalar(1) - alpha);
|
||||
const Scalar w2 =
|
||||
(w1 + xi) / sqrt(Scalar(2) * w1 * xi + xi * xi + Scalar(1));
|
||||
|
||||
const bool is_valid = (z > -w2 * d1);
|
||||
|
||||
const Scalar k = xi * d1 + z;
|
||||
const Scalar kk = k * k;
|
||||
|
||||
const Scalar d2_2 = r2 + kk;
|
||||
const Scalar d2 = sqrt(d2_2);
|
||||
|
||||
const Scalar norm = alpha * d2 + (Scalar(1) - alpha) * k;
|
||||
|
||||
const Scalar mx = x / norm;
|
||||
const Scalar my = y / norm;
|
||||
|
||||
proj[0] = fx * mx + cx;
|
||||
proj[1] = fy * my + cy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
|
||||
const Scalar norm2 = norm * norm;
|
||||
const Scalar xy = x * y;
|
||||
const Scalar tt2 = xi * z / d1 + Scalar(1);
|
||||
|
||||
const Scalar d_norm_d_r2 = (xi * (Scalar(1) - alpha) / d1 +
|
||||
alpha * (xi * k / d1 + Scalar(1)) / d2) /
|
||||
norm2;
|
||||
|
||||
const Scalar tmp2 =
|
||||
((Scalar(1) - alpha) * tt2 + alpha * k * tt2 / d2) / norm2;
|
||||
|
||||
d_proj_d_p3d->setZero();
|
||||
(*d_proj_d_p3d)(0, 0) = fx * (Scalar(1) / norm - xx * d_norm_d_r2);
|
||||
(*d_proj_d_p3d)(1, 0) = -fy * xy * d_norm_d_r2;
|
||||
|
||||
(*d_proj_d_p3d)(0, 1) = -fx * xy * d_norm_d_r2;
|
||||
(*d_proj_d_p3d)(1, 1) = fy * (Scalar(1) / norm - yy * d_norm_d_r2);
|
||||
|
||||
(*d_proj_d_p3d)(0, 2) = -fx * x * tmp2;
|
||||
(*d_proj_d_p3d)(1, 2) = -fy * y * tmp2;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
|
||||
const Scalar norm2 = norm * norm;
|
||||
|
||||
(*d_proj_d_param).setZero();
|
||||
(*d_proj_d_param)(0, 0) = mx;
|
||||
(*d_proj_d_param)(0, 2) = Scalar(1);
|
||||
(*d_proj_d_param)(1, 1) = my;
|
||||
(*d_proj_d_param)(1, 3) = Scalar(1);
|
||||
|
||||
const Scalar tmp4 = (alpha - Scalar(1) - alpha * k / d2) * d1 / norm2;
|
||||
const Scalar tmp5 = (k - d2) / norm2;
|
||||
|
||||
(*d_proj_d_param)(0, 4) = fx * x * tmp4;
|
||||
(*d_proj_d_param)(1, 4) = fy * y * tmp4;
|
||||
|
||||
(*d_proj_d_param)(0, 5) = fx * x * tmp5;
|
||||
(*d_proj_d_param)(1, 5) = fy * y * tmp5;
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobians
|
||||
///
|
||||
/// The unprojection function is computed as follows: \f{align}{
|
||||
/// \pi^{-1}(\mathbf{u}, \mathbf{i}) &=
|
||||
/// \frac{m_z \xi + \sqrt{m_z^2 + (1 - \xi^2) r^2}}{m_z^2 + r^2}
|
||||
/// \begin{bmatrix}
|
||||
/// m_x \\ m_y \\m_z
|
||||
/// \\ \end{bmatrix}-\begin{bmatrix}
|
||||
/// 0 \\ 0 \\ \xi
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ m_x &= \frac{u - c_x}{f_x},
|
||||
/// \\ m_y &= \frac{v - c_y}{f_y},
|
||||
/// \\ r^2 &= m_x^2 + m_y^2,
|
||||
/// \\ m_z &= \frac{1 - \alpha^2 r^2}{\alpha \sqrt{1 - (2 \alpha - 1)
|
||||
/// r^2}
|
||||
/// + 1 - \alpha},
|
||||
/// \f}
|
||||
///
|
||||
/// The valid range of unprojections is \f{align}{
|
||||
/// \Theta &= \begin{cases}
|
||||
/// \mathbb{R}^2 & \mbox{if } \alpha \le 0.5
|
||||
/// \\ \{ \mathbf{u} \in \mathbb{R}^2 ~|~ r^2 \le \frac{1}{2\alpha-1} \} &
|
||||
/// \mbox{if} \alpha > 0.5 \end{cases}
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @param[out] d_p3d_d_param point if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to intrinsic parameters
|
||||
/// @return if unprojection is valid
|
||||
template <class DerivedPoint2D, class DerivedPoint3D,
|
||||
class DerivedJ2D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool unproject(const Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
DerivedJ2D d_p3d_d_proj = nullptr,
|
||||
DerivedJparam d_p3d_d_param = nullptr) const {
|
||||
checkUnprojectionDerivedTypes<DerivedPoint2D, DerivedPoint3D, DerivedJ2D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint2D>::Type proj_eval(proj);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
|
||||
const Scalar& xi = param_[4];
|
||||
const Scalar& alpha = param_[5];
|
||||
|
||||
const Scalar mx = (proj_eval[0] - cx) / fx;
|
||||
const Scalar my = (proj_eval[1] - cy) / fy;
|
||||
|
||||
const Scalar r2 = mx * mx + my * my;
|
||||
|
||||
const bool is_valid =
|
||||
!static_cast<bool>(alpha > Scalar(0.5) &&
|
||||
(r2 >= Scalar(1) / (Scalar(2) * alpha - Scalar(1))));
|
||||
|
||||
const Scalar xi2_2 = alpha * alpha;
|
||||
const Scalar xi1_2 = xi * xi;
|
||||
|
||||
const Scalar sqrt2 = sqrt(Scalar(1) - (Scalar(2) * alpha - Scalar(1)) * r2);
|
||||
|
||||
const Scalar norm2 = alpha * sqrt2 + Scalar(1) - alpha;
|
||||
|
||||
const Scalar mz = (Scalar(1) - xi2_2 * r2) / norm2;
|
||||
const Scalar mz2 = mz * mz;
|
||||
|
||||
const Scalar norm1 = mz2 + r2;
|
||||
const Scalar sqrt1 = sqrt(mz2 + (Scalar(1) - xi1_2) * r2);
|
||||
const Scalar k = (mz * xi + sqrt1) / norm1;
|
||||
|
||||
p3d.setZero();
|
||||
p3d[0] = k * mx;
|
||||
p3d[1] = k * my;
|
||||
p3d[2] = k * mz - xi;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t> ||
|
||||
!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
const Scalar norm2_2 = norm2 * norm2;
|
||||
const Scalar norm1_2 = norm1 * norm1;
|
||||
|
||||
const Scalar d_mz_d_r2 = (Scalar(0.5) * alpha - xi2_2) *
|
||||
(r2 * xi2_2 - Scalar(1)) /
|
||||
(sqrt2 * norm2_2) -
|
||||
xi2_2 / norm2;
|
||||
|
||||
const Scalar d_mz_d_mx = 2 * mx * d_mz_d_r2;
|
||||
const Scalar d_mz_d_my = 2 * my * d_mz_d_r2;
|
||||
|
||||
const Scalar d_k_d_mz =
|
||||
(norm1 * (xi * sqrt1 + mz) - 2 * mz * (mz * xi + sqrt1) * sqrt1) /
|
||||
(norm1_2 * sqrt1);
|
||||
|
||||
const Scalar d_k_d_r2 =
|
||||
(xi * d_mz_d_r2 +
|
||||
Scalar(0.5) / sqrt1 *
|
||||
(Scalar(2) * mz * d_mz_d_r2 + Scalar(1) - xi1_2)) /
|
||||
norm1 -
|
||||
(mz * xi + sqrt1) * (Scalar(2) * mz * d_mz_d_r2 + Scalar(1)) /
|
||||
norm1_2;
|
||||
|
||||
const Scalar d_k_d_mx = d_k_d_r2 * 2 * mx;
|
||||
const Scalar d_k_d_my = d_k_d_r2 * 2 * my;
|
||||
|
||||
constexpr int SIZE_3D = DerivedPoint3D::SizeAtCompileTime;
|
||||
Eigen::Matrix<Scalar, SIZE_3D, 1> c0, c1;
|
||||
|
||||
c0.setZero();
|
||||
c0[0] = (mx * d_k_d_mx + k);
|
||||
c0[1] = my * d_k_d_mx;
|
||||
c0[2] = (mz * d_k_d_mx + k * d_mz_d_mx);
|
||||
c0 /= fx;
|
||||
|
||||
c1.setZero();
|
||||
c1[0] = mx * d_k_d_my;
|
||||
c1[1] = (my * d_k_d_my + k);
|
||||
c1[2] = (mz * d_k_d_my + k * d_mz_d_my);
|
||||
c1 /= fy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_proj);
|
||||
d_p3d_d_proj->col(0) = c0;
|
||||
d_p3d_d_proj->col(1) = c1;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_param);
|
||||
const Scalar d_k_d_xi1 = (mz * sqrt1 - xi * r2) / (sqrt1 * norm1);
|
||||
|
||||
const Scalar d_mz_d_xi2 =
|
||||
((Scalar(1) - r2 * xi2_2) *
|
||||
(r2 * alpha / sqrt2 - sqrt2 + Scalar(1)) / norm2 -
|
||||
Scalar(2) * r2 * alpha) /
|
||||
norm2;
|
||||
|
||||
const Scalar d_k_d_xi2 = d_k_d_mz * d_mz_d_xi2;
|
||||
|
||||
d_p3d_d_param->setZero();
|
||||
(*d_p3d_d_param).col(0) = -c0 * mx;
|
||||
(*d_p3d_d_param).col(1) = -c1 * my;
|
||||
|
||||
(*d_p3d_d_param).col(2) = -c0;
|
||||
(*d_p3d_d_param).col(3) = -c1;
|
||||
|
||||
(*d_p3d_d_param)(0, 4) = mx * d_k_d_xi1;
|
||||
(*d_p3d_d_param)(1, 4) = my * d_k_d_xi1;
|
||||
(*d_p3d_d_param)(2, 4) = mz * d_k_d_xi1 - 1;
|
||||
|
||||
(*d_p3d_d_param)(0, 5) = mx * d_k_d_xi2;
|
||||
(*d_p3d_d_param)(1, 5) = my * d_k_d_xi2;
|
||||
(*d_p3d_d_param)(2, 5) = mz * d_k_d_xi2 + k * d_mz_d_xi2;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_param);
|
||||
UNUSED(d_k_d_mz);
|
||||
}
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// Initializes the camera model to \f$ \left[f_x, f_y, c_x, c_y, 0, 0.5
|
||||
/// \right]^T \f$
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
param_[0] = init[0];
|
||||
param_[1] = init[1];
|
||||
param_[2] = init[2];
|
||||
param_[3] = init[3];
|
||||
param_[4] = 0;
|
||||
param_[5] = 0.5;
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc and clamp the values to the
|
||||
/// valid range
|
||||
///
|
||||
/// @param[in] inc increment vector
|
||||
void operator+=(const VecN& inc) {
|
||||
param_ += inc;
|
||||
param_[4] = std::clamp(param_[4], Scalar(-1), Scalar(1));
|
||||
param_[5] = std::clamp(param_[5], Scalar(0), Scalar(1));
|
||||
}
|
||||
|
||||
/// @brief Returns a const reference to the intrinsic parameters vector
|
||||
///
|
||||
/// The order is following: \f$ \left[f_x, f_y, c_x, c_y, \xi, \alpha
|
||||
/// \right]^T \f$
|
||||
/// @return const reference to the intrinsic parameters vector
|
||||
const VecN& getParam() const { return param_; }
|
||||
|
||||
/// @brief Projections used for unit-tests
|
||||
static Eigen::aligned_vector<DoubleSphereCamera> getTestProjections() {
|
||||
Eigen::aligned_vector<DoubleSphereCamera> res;
|
||||
|
||||
VecN vec1;
|
||||
vec1 << 0.5 * 805, 0.5 * 800, 505, 509, 0.5 * -0.150694, 0.5 * 1.48785;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
VecN param_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
430
thirdparty/basalt-headers/include/basalt/camera/extended_camera.hpp
vendored
Normal file
430
thirdparty/basalt-headers/include/basalt/camera/extended_camera.hpp
vendored
Normal file
@@ -0,0 +1,430 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of extended unified camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/camera_static_assert.hpp>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Extended unified camera model
|
||||
///
|
||||
/// \image html eucm.png
|
||||
/// This model has N=6 parameters \f$ \mathbf{i} = \left[f_x, f_y, c_x, c_y,
|
||||
/// \alpha, \beta \right]^T \f$ with \f$ \alpha \in [0,1], \beta > 0 \f$.
|
||||
/// See \ref project and \ref unproject functions for more details.
|
||||
template <typename Scalar_>
|
||||
class ExtendedUnifiedCamera {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int N = 6; ///< Number of intrinsic parameters.
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using VecN = Eigen::Matrix<Scalar, N, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat2N = Eigen::Matrix<Scalar, 2, N>;
|
||||
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4N = Eigen::Matrix<Scalar, 4, N>;
|
||||
|
||||
/// @brief Default constructor with zero intrinsics
|
||||
ExtendedUnifiedCamera() { param_.setZero(); }
|
||||
|
||||
/// @brief Construct camera model with given vector of intrinsics
|
||||
///
|
||||
/// @param[in] p vector of intrinsic parameters [fx, fy, cx, cy, alpha, beta]
|
||||
explicit ExtendedUnifiedCamera(const VecN& p) { param_ = p; }
|
||||
|
||||
/// @brief Cast to different scalar type
|
||||
template <class Scalar2>
|
||||
ExtendedUnifiedCamera<Scalar2> cast() const {
|
||||
return ExtendedUnifiedCamera<Scalar2>(param_.template cast<Scalar2>());
|
||||
}
|
||||
|
||||
/// @brief Camera model name
|
||||
///
|
||||
/// @return "eucm"
|
||||
static std::string getName() { return "eucm"; }
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobians
|
||||
///
|
||||
/// Projection function is defined as follows:
|
||||
/// \f{align}{
|
||||
/// \pi(\mathbf{x}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// f_x{\frac{x}{\alpha d + (1-\alpha) z}}
|
||||
/// \\ f_y{\frac{y}{\alpha d + (1-\alpha) z}}
|
||||
/// \\ \end{bmatrix}
|
||||
/// +
|
||||
/// \begin{bmatrix}
|
||||
/// c_x
|
||||
/// \\ c_y
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ d &= \sqrt{\beta(x^2 + y^2) + z^2}.
|
||||
/// \f}
|
||||
/// A set of 3D points that results in valid projection is expressed as
|
||||
/// follows: \f{align}{
|
||||
/// \Omega &= \{\mathbf{x} \in \mathbb{R}^3 ~|~ z > -wd \},
|
||||
/// \\ w &= \begin{cases} \frac{\alpha}{1-\alpha}, & \mbox{if } \alpha \le
|
||||
/// 0.5,
|
||||
/// \\ \frac{1-\alpha}{\alpha} & \mbox{if } \alpha > 0.5, \end{cases} \f}
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @param[out] d_proj_d_param point if not nullptr computed Jacobian of
|
||||
/// projection with respect to intrinsic parameters
|
||||
/// @return if projection is valid
|
||||
template <class DerivedPoint3D, class DerivedPoint2D,
|
||||
class DerivedJ3D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool project(const Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
DerivedJ3D d_proj_d_p3d = nullptr,
|
||||
DerivedJparam d_proj_d_param = nullptr) const {
|
||||
checkProjectionDerivedTypes<DerivedPoint3D, DerivedPoint2D, DerivedJ3D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint3D>::Type p3d_eval(p3d);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
const Scalar& alpha = param_[4];
|
||||
const Scalar& beta = param_[5];
|
||||
|
||||
const Scalar& x = p3d_eval[0];
|
||||
const Scalar& y = p3d_eval[1];
|
||||
const Scalar& z = p3d_eval[2];
|
||||
|
||||
const Scalar r2 = x * x + y * y;
|
||||
const Scalar rho2 = beta * r2 + z * z;
|
||||
const Scalar rho = sqrt(rho2);
|
||||
|
||||
const Scalar norm = alpha * rho + (Scalar(1) - alpha) * z;
|
||||
|
||||
const Scalar mx = x / norm;
|
||||
const Scalar my = y / norm;
|
||||
|
||||
proj = Vec2(fx * mx + cx, fy * my + cy);
|
||||
|
||||
// Check if valid
|
||||
const Scalar w = alpha > Scalar(0.5) ? (Scalar(1) - alpha) / alpha
|
||||
: alpha / (Scalar(1) - alpha);
|
||||
const bool is_valid = (z > -w * rho);
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
const Scalar denom = norm * norm * rho;
|
||||
const Scalar mid = -(alpha * beta * x * y);
|
||||
const Scalar add = norm * rho;
|
||||
const Scalar addz = (alpha * z + (Scalar(1) - alpha) * rho);
|
||||
|
||||
(*d_proj_d_p3d).setZero();
|
||||
(*d_proj_d_p3d)(0, 0) = fx * (add - x * x * alpha * beta);
|
||||
(*d_proj_d_p3d)(1, 0) = fy * mid;
|
||||
(*d_proj_d_p3d)(0, 1) = fx * mid;
|
||||
(*d_proj_d_p3d)(1, 1) = fy * (add - y * y * alpha * beta);
|
||||
(*d_proj_d_p3d)(0, 2) = -fx * x * addz;
|
||||
(*d_proj_d_p3d)(1, 2) = -fy * y * addz;
|
||||
|
||||
(*d_proj_d_p3d) /= denom;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
const Scalar norm2 = norm * norm;
|
||||
|
||||
(*d_proj_d_param).setZero();
|
||||
(*d_proj_d_param)(0, 0) = mx;
|
||||
(*d_proj_d_param)(0, 2) = Scalar(1);
|
||||
(*d_proj_d_param)(1, 1) = my;
|
||||
(*d_proj_d_param)(1, 3) = Scalar(1);
|
||||
|
||||
const Scalar tmp_x = -fx * x / norm2;
|
||||
const Scalar tmp_y = -fy * y / norm2;
|
||||
|
||||
const Scalar tmp4 = (rho - z);
|
||||
|
||||
(*d_proj_d_param)(0, 4) = tmp_x * tmp4;
|
||||
(*d_proj_d_param)(1, 4) = tmp_y * tmp4;
|
||||
|
||||
const Scalar tmp5 = Scalar(0.5) * alpha * r2 / rho;
|
||||
|
||||
(*d_proj_d_param)(0, 5) = tmp_x * tmp5;
|
||||
(*d_proj_d_param)(1, 5) = tmp_y * tmp5;
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobians
|
||||
///
|
||||
/// The unprojection function is computed as follows: \f{align}{
|
||||
/// \pi ^ { -1 }(\mathbf{u}, \mathbf{i}) &=
|
||||
/// \frac{1} {
|
||||
/// \sqrt { m_x ^ 2 + m_y ^ 2 + m_z ^ 2 }
|
||||
/// }
|
||||
/// \begin{bmatrix} m_x \\ m_y \\ m_z \\ \end{bmatrix},
|
||||
/// \\ m_x &= \frac{u - c_x}{f_x},
|
||||
/// \\ m_y &= \frac{v - c_y}{f_y},
|
||||
/// \\ r^2 &= m_x^2 + m_y^2,
|
||||
/// \\ m_z &= \frac{1 - \beta \alpha^2 r^2}{\alpha \sqrt{1 - (2\alpha - 1)
|
||||
/// \beta r^2} + (1 - \alpha)}. \f}
|
||||
///
|
||||
/// The valid range of unprojections is \f{align}{
|
||||
/// \Theta &= \begin{cases}
|
||||
/// \mathbb{R}^2 & \mbox{if } \alpha \le 0.5
|
||||
/// \\ \{ \mathbf{u} \in \mathbb{R}^2 ~|~ r^2 \le \frac{1}{\beta(2\alpha-1)}
|
||||
/// \} & \mbox{if } \alpha > 0.5 \end{cases}
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @param[out] d_p3d_d_param point if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to intrinsic parameters
|
||||
/// @return if unprojection is valid
|
||||
template <class DerivedPoint2D, class DerivedPoint3D,
|
||||
class DerivedJ2D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool unproject(const Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
DerivedJ2D d_p3d_d_proj = nullptr,
|
||||
DerivedJparam d_p3d_d_param = nullptr) const {
|
||||
checkUnprojectionDerivedTypes<DerivedPoint2D, DerivedPoint3D, DerivedJ2D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint2D>::Type proj_eval(proj);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
|
||||
const Scalar& alpha = param_[4];
|
||||
const Scalar& beta = param_[5];
|
||||
|
||||
const Scalar mx = (proj_eval[0] - cx) / fx;
|
||||
const Scalar my = (proj_eval[1] - cy) / fy;
|
||||
|
||||
const Scalar r2 = mx * mx + my * my;
|
||||
const Scalar gamma = Scalar(1) - alpha;
|
||||
|
||||
// Check if valid
|
||||
const bool is_valid = !static_cast<bool>(
|
||||
alpha > Scalar(0.5) && (r2 >= Scalar(1) / ((alpha - gamma) * beta)));
|
||||
|
||||
const Scalar tmp1 = (Scalar(1) - alpha * alpha * beta * r2);
|
||||
const Scalar tmp_sqrt = sqrt(Scalar(1) - (alpha - gamma) * beta * r2);
|
||||
const Scalar tmp2 = (alpha * tmp_sqrt + gamma);
|
||||
|
||||
const Scalar k = tmp1 / tmp2;
|
||||
|
||||
const Scalar norm = sqrt(r2 + k * k);
|
||||
|
||||
p3d.setZero();
|
||||
p3d(0) = mx / norm;
|
||||
p3d(1) = my / norm;
|
||||
p3d(2) = k / norm;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t> ||
|
||||
!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
const Scalar norm2 = norm * norm;
|
||||
|
||||
const Scalar tmp2_2 = tmp2 * tmp2;
|
||||
|
||||
const Scalar d_k_d_r2 =
|
||||
Scalar(0.5) * alpha * beta *
|
||||
(-Scalar(2) * alpha * tmp2 + tmp1 * (alpha - gamma) / tmp_sqrt) /
|
||||
tmp2_2;
|
||||
|
||||
const Scalar d_norm_inv_d_r2 =
|
||||
-Scalar(0.5) * (Scalar(1) + Scalar(2) * k * d_k_d_r2) / norm2;
|
||||
|
||||
constexpr int SIZE_3D = DerivedPoint3D::SizeAtCompileTime;
|
||||
Eigen::Matrix<Scalar, SIZE_3D, 1> c0, c1;
|
||||
|
||||
c0.setZero();
|
||||
c0[0] = (1 + 2 * mx * mx * d_norm_inv_d_r2);
|
||||
c0[1] = (2 * my * mx * d_norm_inv_d_r2);
|
||||
c0[2] = 2 * mx * (k * d_norm_inv_d_r2 + d_k_d_r2);
|
||||
c0 /= fx * norm;
|
||||
|
||||
c1.setZero();
|
||||
c1[0] = (2 * my * mx * d_norm_inv_d_r2);
|
||||
c1[1] = (1 + 2 * my * my * d_norm_inv_d_r2);
|
||||
c1[2] = 2 * my * (k * d_norm_inv_d_r2 + d_k_d_r2);
|
||||
c1 /= fy * norm;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_proj);
|
||||
d_p3d_d_proj->col(0) = c0;
|
||||
d_p3d_d_proj->col(1) = c1;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_param);
|
||||
d_p3d_d_param->setZero();
|
||||
|
||||
(*d_p3d_d_param).col(0) = -c0 * mx;
|
||||
(*d_p3d_d_param).col(1) = -c1 * my;
|
||||
|
||||
(*d_p3d_d_param).col(2) = -c0;
|
||||
(*d_p3d_d_param).col(3) = -c1;
|
||||
|
||||
const Scalar d_k_d_alpha =
|
||||
(-Scalar(2) * alpha * beta * r2 * tmp2 -
|
||||
(tmp_sqrt - alpha * beta * r2 / tmp_sqrt - Scalar(1)) * tmp1) /
|
||||
tmp2_2;
|
||||
|
||||
const Scalar d_k_d_beta =
|
||||
alpha * r2 *
|
||||
(Scalar(0.5) * tmp1 * (alpha - gamma) / tmp_sqrt - alpha * tmp2) /
|
||||
tmp2_2;
|
||||
|
||||
const Scalar d_norm_inv_d_k = -k / norm2;
|
||||
|
||||
(*d_p3d_d_param)(0, 4) = mx * d_norm_inv_d_k * d_k_d_alpha;
|
||||
(*d_p3d_d_param)(1, 4) = my * d_norm_inv_d_k * d_k_d_alpha;
|
||||
(*d_p3d_d_param)(2, 4) = (k * d_norm_inv_d_k + 1) * d_k_d_alpha;
|
||||
d_p3d_d_param->col(4) /= norm;
|
||||
|
||||
(*d_p3d_d_param)(0, 5) = mx * d_norm_inv_d_k * d_k_d_beta;
|
||||
(*d_p3d_d_param)(1, 5) = my * d_norm_inv_d_k * d_k_d_beta;
|
||||
(*d_p3d_d_param)(2, 5) = (k * d_norm_inv_d_k + 1) * d_k_d_beta;
|
||||
d_p3d_d_param->col(5) /= norm;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// Initializes the camera model to \f$ \left[f_x, f_y, c_x, c_y, 0.5, 1
|
||||
/// \right]^T \f$
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
param_[0] = init[0];
|
||||
param_[1] = init[1];
|
||||
param_[2] = init[2];
|
||||
param_[3] = init[3];
|
||||
param_[4] = 0.5;
|
||||
param_[5] = 1;
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc and clamp the values to the
|
||||
/// valid range
|
||||
///
|
||||
/// @param[in] inc increment vector
|
||||
void operator+=(const VecN& inc) {
|
||||
param_ += inc;
|
||||
|
||||
// alpha in [0, 1], beta > 0
|
||||
param_[4] = std::clamp(param_[4], Scalar(0), Scalar(1));
|
||||
if (param_[5] < Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
param_[5] = Sophus::Constants<Scalar>::epsilonSqrt();
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Returns a const reference to the intrinsic parameters vector
|
||||
///
|
||||
/// The order is following: \f$ \left[f_x, f_y, c_x, c_y, \alpha, \beta
|
||||
/// \right]^T \f$
|
||||
/// @return const reference to the intrinsic parameters vector
|
||||
const VecN& getParam() const { return param_; }
|
||||
|
||||
/// @brief Projections used for unit-tests
|
||||
static Eigen::aligned_vector<ExtendedUnifiedCamera> getTestProjections() {
|
||||
Eigen::aligned_vector<ExtendedUnifiedCamera> res;
|
||||
|
||||
VecN vec1;
|
||||
|
||||
// Euroc
|
||||
vec1 << 460.76484651566468, 459.4051018049483, 365.8937161309615,
|
||||
249.33499869752445, 0.5903365915227143, 1.127468196965374;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
// TUM VI 512
|
||||
vec1 << 191.14799816648748, 191.13150946585135, 254.95857715233118,
|
||||
256.8815466235898, 0.6291060871161842, 1.0418067403139693;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Resolutions used for unit-tests
|
||||
static Eigen::aligned_vector<Eigen::Vector2i> getTestResolutions() {
|
||||
Eigen::aligned_vector<Eigen::Vector2i> res;
|
||||
|
||||
res.emplace_back(752, 480);
|
||||
res.emplace_back(512, 512);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
VecN param_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
435
thirdparty/basalt-headers/include/basalt/camera/fov_camera.hpp
vendored
Normal file
435
thirdparty/basalt-headers/include/basalt/camera/fov_camera.hpp
vendored
Normal file
@@ -0,0 +1,435 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of field-of-view camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/camera_static_assert.hpp>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Field-of-View camera model
|
||||
///
|
||||
/// \image html fov.png
|
||||
/// This model has N=5 parameters \f$ \mathbf{i} = \left[f_x, f_y, c_x, c_y,
|
||||
/// w \right]^T \f$. See \ref project and \ref unproject
|
||||
/// functions for more details.
|
||||
template <typename Scalar_ = double>
|
||||
class FovCamera {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int N = 5; ///< Number of intrinsic parameters.
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using VecN = Eigen::Matrix<Scalar, N, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat2N = Eigen::Matrix<Scalar, 2, N>;
|
||||
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4N = Eigen::Matrix<Scalar, 4, N>;
|
||||
|
||||
using Mat44 = Eigen::Matrix<Scalar, 4, 4>;
|
||||
|
||||
/// @brief Default constructor with zero intrinsics
|
||||
FovCamera() { param_.setZero(); }
|
||||
|
||||
/// @brief Construct camera model with given vector of intrinsics
|
||||
///
|
||||
/// @param[in] p vector of intrinsic parameters [fx, fy, cx, cy, w]
|
||||
explicit FovCamera(const VecN& p) { param_ = p; }
|
||||
|
||||
/// @brief Cast to different scalar type
|
||||
template <class Scalar2>
|
||||
FovCamera<Scalar2> cast() const {
|
||||
return FovCamera<Scalar2>(param_.template cast<Scalar2>());
|
||||
}
|
||||
|
||||
/// @brief Camera model name
|
||||
///
|
||||
/// @return "fov"
|
||||
static std::string getName() { return "fov"; }
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobians
|
||||
///
|
||||
/// Projection function is defined as follows:
|
||||
/// \f{align}{
|
||||
/// \DeclareMathOperator{\atantwo}{atan2}
|
||||
/// \pi(\mathbf{x}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix} f_x r_d \frac{x} { r_u }
|
||||
/// \\ f_y r_d \frac{y} { r_u }
|
||||
/// \\ \end{bmatrix}
|
||||
/// +
|
||||
/// \begin{bmatrix}
|
||||
/// c_x
|
||||
/// \\ c_y
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ r_u &= \sqrt{x^2 + y^2},
|
||||
/// \\ r_d &= \frac{\atantwo(2 r_u \tan{\frac{w}{2}}, z)}{w}.
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @param[out] d_proj_d_param point if not nullptr computed Jacobian of
|
||||
/// projection with respect to intrinsic parameters
|
||||
/// @return if projection is valid
|
||||
template <class DerivedPoint3D, class DerivedPoint2D,
|
||||
class DerivedJ3D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool project(const Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
DerivedJ3D d_proj_d_p3d = nullptr,
|
||||
DerivedJparam d_proj_d_param = nullptr) const {
|
||||
checkProjectionDerivedTypes<DerivedPoint3D, DerivedPoint2D, DerivedJ3D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint3D>::Type p3d_eval(p3d);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
const Scalar& w = param_[4];
|
||||
|
||||
const Scalar& x = p3d_eval[0];
|
||||
const Scalar& y = p3d_eval[1];
|
||||
const Scalar& z = p3d_eval[2];
|
||||
|
||||
Scalar r2 = x * x + y * y;
|
||||
Scalar r = sqrt(r2);
|
||||
|
||||
Scalar z2 = z * z;
|
||||
|
||||
const Scalar tanwhalf = std::tan(w / 2);
|
||||
const Scalar atan_wrd = std::atan2(2 * tanwhalf * r, z);
|
||||
|
||||
Scalar rd = Scalar(1);
|
||||
Scalar d_rd_d_w = Scalar(0);
|
||||
Scalar d_rd_d_x = Scalar(0);
|
||||
Scalar d_rd_d_y = Scalar(0);
|
||||
Scalar d_rd_d_z = Scalar(0);
|
||||
|
||||
Scalar tmp1 = Scalar(1) / std::cos(w / 2);
|
||||
Scalar d_tanwhalf_d_w = Scalar(0.5) * tmp1 * tmp1;
|
||||
Scalar tmp = (z2 + Scalar(4) * tanwhalf * tanwhalf * r2);
|
||||
Scalar d_atan_wrd_d_w = Scalar(2) * r * d_tanwhalf_d_w * z / tmp;
|
||||
|
||||
bool is_valid = true;
|
||||
if (w > Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
if (r2 < Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
if (z < Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
is_valid = false;
|
||||
}
|
||||
|
||||
rd = Scalar(2) * tanwhalf / w;
|
||||
d_rd_d_w = Scalar(2) * (d_tanwhalf_d_w * w - tanwhalf) / (w * w);
|
||||
} else {
|
||||
rd = atan_wrd / (r * w);
|
||||
d_rd_d_w = (d_atan_wrd_d_w * w - atan_wrd) / (r * w * w);
|
||||
|
||||
const Scalar d_r_d_x = x / r;
|
||||
const Scalar d_r_d_y = y / r;
|
||||
|
||||
const Scalar d_atan_wrd_d_x = Scalar(2) * tanwhalf * d_r_d_x * z / tmp;
|
||||
const Scalar d_atan_wrd_d_y = Scalar(2) * tanwhalf * d_r_d_y * z / tmp;
|
||||
const Scalar d_atan_wrd_d_z = -Scalar(2) * tanwhalf * r / tmp;
|
||||
|
||||
d_rd_d_x = (d_atan_wrd_d_x * r - d_r_d_x * atan_wrd) / (r * r * w);
|
||||
d_rd_d_y = (d_atan_wrd_d_y * r - d_r_d_y * atan_wrd) / (r * r * w);
|
||||
d_rd_d_z = d_atan_wrd_d_z / (r * w);
|
||||
}
|
||||
}
|
||||
|
||||
const Scalar mx = x * rd;
|
||||
const Scalar my = y * rd;
|
||||
|
||||
proj[0] = fx * mx + cx;
|
||||
proj[1] = fy * my + cy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
d_proj_d_p3d->setZero();
|
||||
(*d_proj_d_p3d)(0, 0) = fx * (d_rd_d_x * x + rd);
|
||||
(*d_proj_d_p3d)(0, 1) = fx * d_rd_d_y * x;
|
||||
(*d_proj_d_p3d)(0, 2) = fx * d_rd_d_z * x;
|
||||
|
||||
(*d_proj_d_p3d)(1, 0) = fy * d_rd_d_x * y;
|
||||
(*d_proj_d_p3d)(1, 1) = fy * (d_rd_d_y * y + rd);
|
||||
(*d_proj_d_p3d)(1, 2) = fy * d_rd_d_z * y;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
UNUSED(d_rd_d_x);
|
||||
UNUSED(d_rd_d_y);
|
||||
UNUSED(d_rd_d_z);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
|
||||
d_proj_d_param->setZero();
|
||||
(*d_proj_d_param)(0, 0) = mx;
|
||||
(*d_proj_d_param)(0, 2) = Scalar(1);
|
||||
(*d_proj_d_param)(1, 1) = my;
|
||||
(*d_proj_d_param)(1, 3) = Scalar(1);
|
||||
|
||||
(*d_proj_d_param)(0, 4) = fx * x * d_rd_d_w;
|
||||
(*d_proj_d_param)(1, 4) = fy * y * d_rd_d_w;
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
UNUSED(d_rd_d_w);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobians
|
||||
///
|
||||
/// The unprojection function is computed as follows: \f{align}{
|
||||
/// \pi^{-1}(\mathbf{u}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// m_x \frac{\sin(r_d w)}{ 2 r_d \tan{\frac{w}{2}}}
|
||||
/// \\ m_y \frac{\sin(r_d w)}{ 2 r_d \tan{\frac{w}{2}}}
|
||||
/// \\ \cos(r_d w)
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ m_x &= \frac{u - c_x}{f_x},
|
||||
/// \\ m_y &= \frac{v - c_y}{f_y},
|
||||
/// \\ r_d &= \sqrt{m_x^2 + m_y^2}.
|
||||
/// \f}
|
||||
///
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @param[out] d_p3d_d_param point if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to intrinsic parameters
|
||||
/// @return if unprojection is valid
|
||||
template <class DerivedPoint2D, class DerivedPoint3D,
|
||||
class DerivedJ2D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool unproject(const Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
DerivedJ2D d_p3d_d_proj = nullptr,
|
||||
DerivedJparam d_p3d_d_param = nullptr) const {
|
||||
checkUnprojectionDerivedTypes<DerivedPoint2D, DerivedPoint3D, DerivedJ2D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint2D>::Type proj_eval(proj);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
const Scalar& w = param_[4];
|
||||
|
||||
const Scalar tan_w_2 = std::tan(w / Scalar(2));
|
||||
const Scalar mul2tanwby2 = tan_w_2 * Scalar(2);
|
||||
|
||||
const Scalar mx = (proj_eval[0] - cx) / fx;
|
||||
const Scalar my = (proj_eval[1] - cy) / fy;
|
||||
|
||||
const Scalar rd = sqrt(mx * mx + my * my);
|
||||
|
||||
Scalar ru = Scalar(1);
|
||||
Scalar sin_rd_w = Scalar(0);
|
||||
Scalar cos_rd_w = Scalar(1);
|
||||
|
||||
Scalar d_ru_d_rd = Scalar(0);
|
||||
|
||||
Scalar rd_inv = Scalar(1);
|
||||
|
||||
if (mul2tanwby2 > Sophus::Constants<Scalar>::epsilonSqrt() &&
|
||||
rd > Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
sin_rd_w = std::sin(rd * w);
|
||||
cos_rd_w = std::cos(rd * w);
|
||||
ru = sin_rd_w / (rd * mul2tanwby2);
|
||||
|
||||
rd_inv = Scalar(1) / rd;
|
||||
|
||||
d_ru_d_rd =
|
||||
(w * cos_rd_w * rd - sin_rd_w) * rd_inv * rd_inv / mul2tanwby2;
|
||||
}
|
||||
|
||||
p3d.setZero();
|
||||
p3d[0] = mx * ru;
|
||||
p3d[1] = my * ru;
|
||||
p3d[2] = cos_rd_w;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t> ||
|
||||
!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
constexpr int SIZE_3D = DerivedPoint3D::SizeAtCompileTime;
|
||||
Eigen::Matrix<Scalar, SIZE_3D, 1> c0, c1;
|
||||
|
||||
c0.setZero();
|
||||
c0(0) = (ru + mx * d_ru_d_rd * mx * rd_inv) / fx;
|
||||
c0(1) = my * d_ru_d_rd * mx * rd_inv / fx;
|
||||
c0(2) = -sin_rd_w * w * mx * rd_inv / fx;
|
||||
|
||||
c1.setZero();
|
||||
c1(0) = my * d_ru_d_rd * mx * rd_inv / fy;
|
||||
c1(1) = (ru + my * d_ru_d_rd * my * rd_inv) / fy;
|
||||
c1(2) = -sin_rd_w * w * my * rd_inv / fy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_proj);
|
||||
d_p3d_d_proj->col(0) = c0;
|
||||
d_p3d_d_proj->col(1) = c1;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_param);
|
||||
d_p3d_d_param->setZero();
|
||||
|
||||
d_p3d_d_param->col(2) = -c0;
|
||||
d_p3d_d_param->col(3) = -c1;
|
||||
|
||||
d_p3d_d_param->col(0) = -c0 * mx;
|
||||
d_p3d_d_param->col(1) = -c1 * my;
|
||||
|
||||
Scalar tmp = (cos_rd_w - (tan_w_2 * tan_w_2 + Scalar(1)) * sin_rd_w *
|
||||
rd_inv / (2 * tan_w_2)) /
|
||||
mul2tanwby2;
|
||||
|
||||
(*d_p3d_d_param)(0, 4) = mx * tmp;
|
||||
(*d_p3d_d_param)(1, 4) = my * tmp;
|
||||
(*d_p3d_d_param)(2, 4) = -sin_rd_w * rd;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_param);
|
||||
UNUSED(d_ru_d_rd);
|
||||
}
|
||||
|
||||
Scalar norm = p3d.norm();
|
||||
Scalar norm_inv = Scalar(1) / norm;
|
||||
Scalar norm_inv2 = norm_inv * norm_inv;
|
||||
Scalar norm_inv3 = norm_inv2 * norm_inv;
|
||||
|
||||
Eigen::Matrix<Scalar, SIZE_3D, SIZE_3D> d_p_norm_d_p;
|
||||
d_p_norm_d_p.setZero();
|
||||
|
||||
d_p_norm_d_p(0, 0) = norm_inv * (Scalar(1) - p3d[0] * p3d[0] * norm_inv2);
|
||||
d_p_norm_d_p(1, 0) = -p3d[1] * p3d[0] * norm_inv3;
|
||||
d_p_norm_d_p(2, 0) = -p3d[2] * p3d[0] * norm_inv3;
|
||||
|
||||
d_p_norm_d_p(0, 1) = -p3d[1] * p3d[0] * norm_inv3;
|
||||
d_p_norm_d_p(1, 1) = norm_inv * (Scalar(1) - p3d[1] * p3d[1] * norm_inv2);
|
||||
d_p_norm_d_p(2, 1) = -p3d[1] * p3d[2] * norm_inv3;
|
||||
|
||||
d_p_norm_d_p(0, 2) = -p3d[2] * p3d[0] * norm_inv3;
|
||||
d_p_norm_d_p(1, 2) = -p3d[2] * p3d[1] * norm_inv3;
|
||||
d_p_norm_d_p(2, 2) = norm_inv * (Scalar(1) - p3d[2] * p3d[2] * norm_inv2);
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t>) {
|
||||
(*d_p3d_d_proj) = d_p_norm_d_p * (*d_p3d_d_proj);
|
||||
}
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
(*d_p3d_d_param) = d_p_norm_d_p * (*d_p3d_d_param);
|
||||
}
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
UNUSED(d_p3d_d_param);
|
||||
UNUSED(d_ru_d_rd);
|
||||
}
|
||||
|
||||
p3d /= p3d.norm();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// Initializes the camera model to \f$ \left[f_x, f_y, c_x, c_y, 1
|
||||
/// \right]^T \f$
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
param_[0] = init[0];
|
||||
param_[1] = init[1];
|
||||
param_[2] = init[2];
|
||||
param_[3] = init[3];
|
||||
param_[4] = 1;
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc
|
||||
///
|
||||
/// @param[in] inc increment vector
|
||||
void operator+=(const VecN& inc) { param_ += inc; }
|
||||
|
||||
/// @brief Returns a const reference to the intrinsic parameters vector
|
||||
///
|
||||
/// The order is following: \f$ \left[f_x, f_y, c_x, c_y, k1, k2, k3, k4
|
||||
/// \right]^T \f$
|
||||
/// @return const reference to the intrinsic parameters vector
|
||||
const VecN& getParam() const { return param_; }
|
||||
|
||||
/// @brief Projections used for unit-tests
|
||||
static Eigen::aligned_vector<FovCamera> getTestProjections() {
|
||||
Eigen::aligned_vector<FovCamera> res;
|
||||
|
||||
VecN vec1;
|
||||
|
||||
// Euroc
|
||||
vec1 << 379.045, 379.008, 505.512, 509.969, 0.9259487501905697;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Resolutions used for unit-tests
|
||||
static Eigen::aligned_vector<Eigen::Vector2i> getTestResolutions() {
|
||||
Eigen::aligned_vector<Eigen::Vector2i> res;
|
||||
|
||||
res.emplace_back(752, 480);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
VecN param_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
364
thirdparty/basalt-headers/include/basalt/camera/generic_camera.hpp
vendored
Normal file
364
thirdparty/basalt-headers/include/basalt/camera/generic_camera.hpp
vendored
Normal file
@@ -0,0 +1,364 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of generic camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/bal_camera.hpp>
|
||||
#include <basalt/camera/double_sphere_camera.hpp>
|
||||
#include <basalt/camera/extended_camera.hpp>
|
||||
#include <basalt/camera/fov_camera.hpp>
|
||||
#include <basalt/camera/kannala_brandt_camera4.hpp>
|
||||
#include <basalt/camera/pinhole_camera.hpp>
|
||||
#include <basalt/camera/unified_camera.hpp>
|
||||
|
||||
#include <variant>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Generic camera model that can store different camera models
|
||||
///
|
||||
/// Particular class of camera model is stored as \ref variant and can be casted
|
||||
/// to specific type using std::visit.
|
||||
template <typename Scalar_>
|
||||
class GenericCamera {
|
||||
using Scalar = Scalar_;
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
|
||||
|
||||
using VariantT =
|
||||
std::variant<ExtendedUnifiedCamera<Scalar>, DoubleSphereCamera<Scalar>,
|
||||
KannalaBrandtCamera4<Scalar>, UnifiedCamera<Scalar>,
|
||||
PinholeCamera<Scalar>>; ///< Possible variants of camera
|
||||
///< models.
|
||||
|
||||
public:
|
||||
/// @brief Cast to different scalar type
|
||||
template <typename Scalar2>
|
||||
inline GenericCamera<Scalar2> cast() const {
|
||||
GenericCamera<Scalar2> res;
|
||||
std::visit([&](const auto& v) { res.variant = v.template cast<Scalar2>(); },
|
||||
variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Number of intrinsic parameters
|
||||
inline int getN() const {
|
||||
int res;
|
||||
std::visit([&](const auto& v) { res = v.N; }, variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Camera model name
|
||||
inline std::string getName() const {
|
||||
std::string res;
|
||||
std::visit([&](const auto& v) { res = v.getName(); }, variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
std::visit([&](auto& v) { return v.setFromInit(init); }, variant);
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc and if necessary clamp the
|
||||
/// values to the valid range
|
||||
inline void applyInc(const VecX& inc) {
|
||||
std::visit([&](auto& v) { return v += inc; }, variant);
|
||||
}
|
||||
|
||||
/// @brief Returns a vector of intrinsic parameters
|
||||
///
|
||||
/// The order of parameters depends on the stored model.
|
||||
/// @return vector of intrinsic parameters vector
|
||||
inline VecX getParam() const {
|
||||
VecX res;
|
||||
std::visit([&](const auto& cam) { res = cam.getParam(); }, variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Project a single point and optionally compute Jacobian
|
||||
///
|
||||
/// **SLOW** function, as it requires vtable lookup for every projection.
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @return if projection is valid
|
||||
template <typename DerivedJ3DPtr = std::nullptr_t>
|
||||
inline bool project(const Vec4& p3d, Vec2& proj,
|
||||
DerivedJ3DPtr d_proj_d_p3d = nullptr) const {
|
||||
if constexpr (!std::is_same_v<DerivedJ3DPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJ3DPtr>);
|
||||
using DerivedJ3D = typename std::remove_pointer<DerivedJ3DPtr>::type;
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedJ3D, 2, 4);
|
||||
}
|
||||
|
||||
bool res;
|
||||
std::visit(
|
||||
[&](const auto& cam) { res = cam.project(p3d, proj, d_proj_d_p3d); },
|
||||
variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Unproject a single point and optionally compute Jacobian
|
||||
///
|
||||
/// **SLOW** function, as it requires vtable lookup for every unprojection.
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @return if unprojection is valid
|
||||
template <typename DerivedJ2DPtr = std::nullptr_t>
|
||||
inline bool unproject(const Vec2& proj, Vec4& p3d,
|
||||
DerivedJ2DPtr d_p3d_d_proj = nullptr) const {
|
||||
if constexpr (!std::is_same_v<DerivedJ2DPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJ2DPtr>);
|
||||
using DerivedJ2D = typename std::remove_pointer<DerivedJ2DPtr>::type;
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedJ2D, 4, 2);
|
||||
}
|
||||
|
||||
bool res;
|
||||
std::visit(
|
||||
[&](const auto& cam) { res = cam.unproject(proj, p3d, d_p3d_d_proj); },
|
||||
variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Project a single point and optionally compute Jacobian
|
||||
///
|
||||
/// **SLOW** function, as it requires vtable lookup for every projection.
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @return if projection is valid
|
||||
template <typename DerivedJ3DPtr = std::nullptr_t>
|
||||
inline bool project(const Vec3& p3d, Vec2& proj,
|
||||
DerivedJ3DPtr d_proj_d_p3d = nullptr) const {
|
||||
if constexpr (!std::is_same_v<DerivedJ3DPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJ3DPtr>);
|
||||
using DerivedJ3D = typename std::remove_pointer<DerivedJ3DPtr>::type;
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedJ3D, 2, 3);
|
||||
}
|
||||
|
||||
bool res;
|
||||
std::visit(
|
||||
[&](const auto& cam) { res = cam.project(p3d, proj, d_proj_d_p3d); },
|
||||
variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Unproject a single point and optionally compute Jacobian
|
||||
///
|
||||
/// **SLOW** function, as it requires vtable lookup for every unprojection.
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @return if unprojection is valid
|
||||
template <typename DerivedJ2DPtr = std::nullptr_t>
|
||||
inline bool unproject(const Vec2& proj, Vec3& p3d,
|
||||
DerivedJ2DPtr d_p3d_d_proj = nullptr) const {
|
||||
if constexpr (!std::is_same_v<DerivedJ2DPtr, std::nullptr_t>) {
|
||||
static_assert(std::is_pointer_v<DerivedJ2DPtr>);
|
||||
using DerivedJ2D = typename std::remove_pointer<DerivedJ2DPtr>::type;
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedJ2D, 3, 2);
|
||||
}
|
||||
|
||||
bool res;
|
||||
std::visit(
|
||||
[&](const auto& cam) { res = cam.unproject(proj, p3d, d_p3d_d_proj); },
|
||||
variant);
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Project a vector of points
|
||||
///
|
||||
/// @param[in] p3d points to project
|
||||
/// @param[in] T_c_w transformation from world to camera frame that should be
|
||||
/// applied to points before projection
|
||||
/// @param[out] proj results of projection
|
||||
/// @param[out] proj_success if projection is valid
|
||||
inline void project(const Eigen::aligned_vector<Vec3>& p3d, const Mat4& T_c_w,
|
||||
Eigen::aligned_vector<Vec2>& proj,
|
||||
std::vector<bool>& proj_success) const {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
proj.resize(p3d.size());
|
||||
proj_success.resize(p3d.size());
|
||||
for (size_t i = 0; i < p3d.size(); i++) {
|
||||
proj_success[i] =
|
||||
cam.project(T_c_w * p3d[i].homogeneous(), proj[i]);
|
||||
}
|
||||
},
|
||||
variant);
|
||||
}
|
||||
|
||||
/// @brief Project a vector of points
|
||||
///
|
||||
/// @param[in] p3d points to project
|
||||
/// @param[in] T_c_w transformation from world to camera frame that should be
|
||||
/// applied to points before projection
|
||||
/// @param[out] proj results of projection
|
||||
/// @param[out] proj_success if projection is valid
|
||||
inline void project(const Eigen::aligned_vector<Vec4>& p3d, const Mat4& T_c_w,
|
||||
Eigen::aligned_vector<Vec2>& proj,
|
||||
std::vector<bool>& proj_success) const {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
proj.resize(p3d.size());
|
||||
proj_success.resize(p3d.size());
|
||||
for (size_t i = 0; i < p3d.size(); i++) {
|
||||
proj_success[i] = cam.project(T_c_w * p3d[i], proj[i]);
|
||||
}
|
||||
},
|
||||
variant);
|
||||
}
|
||||
|
||||
/// @brief Project a vector of points
|
||||
///
|
||||
/// @param[in] p3d points to project
|
||||
/// @param[out] proj results of projection
|
||||
/// @param[out] proj_success if projection is valid
|
||||
inline void project(const Eigen::aligned_vector<Vec4>& p3d,
|
||||
Eigen::aligned_vector<Vec2>& proj,
|
||||
std::vector<bool>& proj_success) const {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
proj.resize(p3d.size());
|
||||
proj_success.resize(p3d.size());
|
||||
for (size_t i = 0; i < p3d.size(); i++) {
|
||||
proj_success[i] = cam.project(p3d[i], proj[i]);
|
||||
}
|
||||
},
|
||||
variant);
|
||||
}
|
||||
|
||||
/// @brief Project a vector of points, compute polar and azimuthal angles
|
||||
///
|
||||
/// @param[in] p3d points to project
|
||||
/// @param[in] T_c_w transformation from world to camera frame that should be
|
||||
/// applied to points before projection
|
||||
/// @param[out] proj results of projection
|
||||
/// @param[out] proj_success if projection is valid
|
||||
inline void project(
|
||||
const Eigen::aligned_vector<Vec4>& p3d, const Mat4& T_c_w,
|
||||
Eigen::aligned_vector<Vec2>& proj, std::vector<bool>& proj_success,
|
||||
Eigen::aligned_vector<Vec2>& polar_azimuthal_angle) const {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
proj.resize(p3d.size());
|
||||
proj_success.resize(p3d.size());
|
||||
polar_azimuthal_angle.resize(p3d.size());
|
||||
for (size_t i = 0; i < p3d.size(); i++) {
|
||||
Vec4 p3dt = T_c_w * p3d[i];
|
||||
|
||||
proj_success[i] = cam.project(p3dt, proj[i]);
|
||||
|
||||
Scalar r2 = p3dt[0] * p3dt[0] + p3dt[1] * p3dt[1];
|
||||
Scalar r = std::sqrt(r2);
|
||||
polar_azimuthal_angle[i][0] = std::atan2(r, p3dt[2]);
|
||||
polar_azimuthal_angle[i][1] = std::atan2(p3dt[0], p3dt[1]);
|
||||
}
|
||||
},
|
||||
variant);
|
||||
}
|
||||
|
||||
/// @brief Unproject a vector of points
|
||||
///
|
||||
/// @param[in] proj points to unproject
|
||||
/// @param[out] p3d results of unprojection
|
||||
/// @param[out] unproj_success if unprojection is valid
|
||||
inline void unproject(const Eigen::aligned_vector<Vec2>& proj,
|
||||
Eigen::aligned_vector<Vec4>& p3d,
|
||||
std::vector<bool>& unproj_success) const {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
p3d.resize(proj.size());
|
||||
unproj_success.resize(proj.size());
|
||||
for (size_t i = 0; i < p3d.size(); i++) {
|
||||
unproj_success[i] = cam.unproject(proj[i], p3d[i]);
|
||||
}
|
||||
},
|
||||
variant);
|
||||
}
|
||||
|
||||
/// @brief Construct a particular type of camera model from name
|
||||
static GenericCamera<Scalar> fromString(const std::string& name) {
|
||||
GenericCamera<Scalar> res;
|
||||
|
||||
constexpr size_t VARIANT_SIZE = std::variant_size<VariantT>::value;
|
||||
visitAllTypes<VARIANT_SIZE - 1>(res, name);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
VariantT variant;
|
||||
|
||||
private:
|
||||
/// @brief Iterate over all possible types of the variant and construct that
|
||||
/// type that has a matching name
|
||||
template <int I>
|
||||
static void visitAllTypes(GenericCamera<Scalar>& res,
|
||||
const std::string& name) {
|
||||
if constexpr (I >= 0) {
|
||||
using cam_t = typename std::variant_alternative<I, VariantT>::type;
|
||||
if (cam_t::getName() == name) {
|
||||
cam_t val;
|
||||
res.variant = val;
|
||||
}
|
||||
visitAllTypes<I - 1>(res, name);
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace basalt
|
||||
531
thirdparty/basalt-headers/include/basalt/camera/kannala_brandt_camera4.hpp
vendored
Normal file
531
thirdparty/basalt-headers/include/basalt/camera/kannala_brandt_camera4.hpp
vendored
Normal file
@@ -0,0 +1,531 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
|
||||
@file
|
||||
@brief Implementation of Kannala-Brandt camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/camera_static_assert.hpp>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Kannala-Brandt camera model
|
||||
///
|
||||
/// \image html kb.png
|
||||
/// The displacement of the projection from the optical center is proportional
|
||||
/// to \f$d(\theta)\f$, which is a polynomial function of the angle between the
|
||||
/// point and optical axis \f$\theta\f$.
|
||||
/// This model has N=8 parameters \f$ \mathbf{i} = \left[f_x, f_y, c_x, c_y,
|
||||
/// k_1, k_2, k_3, k_4 \right]^T \f$. See \ref project and \ref unproject
|
||||
/// functions for more details. This model corresponds to fisheye camera model
|
||||
/// in OpenCV.
|
||||
template <typename Scalar_ = double>
|
||||
class KannalaBrandtCamera4 {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int N = 8; ///< Number of intrinsic parameters.
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using VecN = Eigen::Matrix<Scalar, N, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat2N = Eigen::Matrix<Scalar, 2, N>;
|
||||
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4N = Eigen::Matrix<Scalar, 4, N>;
|
||||
|
||||
/// @brief Default constructor with zero intrinsics
|
||||
KannalaBrandtCamera4() { param_.setZero(); }
|
||||
|
||||
/// @brief Construct camera model with given vector of intrinsics
|
||||
///
|
||||
/// @param[in] p vector of intrinsic parameters [fx, fy, cx, cy, k1, k2, k3,
|
||||
/// k4]
|
||||
explicit KannalaBrandtCamera4(const VecN& p) { param_ = p; }
|
||||
|
||||
/// @brief Camera model name
|
||||
///
|
||||
/// @return "kb4"
|
||||
static std::string getName() { return "kb4"; }
|
||||
|
||||
/// @brief Cast to different scalar type
|
||||
template <class Scalar2>
|
||||
KannalaBrandtCamera4<Scalar2> cast() const {
|
||||
return KannalaBrandtCamera4<Scalar2>(param_.template cast<Scalar2>());
|
||||
}
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobians
|
||||
///
|
||||
/// Projection function is defined as follows:
|
||||
/// \f{align}{
|
||||
/// \DeclareMathOperator{\atantwo}{atan2}
|
||||
/// \pi(\mathbf{x}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// f_x ~ d(\theta) ~ \frac{x}{r}
|
||||
/// \\ f_y ~ d(\theta) ~ \frac{y}{r}
|
||||
/// \\ \end{bmatrix}
|
||||
/// +
|
||||
/// \begin{bmatrix}
|
||||
/// c_x
|
||||
/// \\ c_y
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ r &= \sqrt{x^2 + y^2},
|
||||
/// \\ \theta &= \atantwo(r, z),
|
||||
/// \\ d(\theta) &= \theta + k_1 \theta^3 + k_2 \theta^5 + k_3 \theta^7 + k_4
|
||||
/// \theta^9.
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @param[out] d_proj_d_param point if not nullptr computed Jacobian of
|
||||
/// projection with respect to intrinsic parameters
|
||||
/// @return if projection is valid
|
||||
template <class DerivedPoint3D, class DerivedPoint2D,
|
||||
class DerivedJ3D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool project(const Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
DerivedJ3D d_proj_d_p3d = nullptr,
|
||||
DerivedJparam d_proj_d_param = nullptr) const {
|
||||
checkProjectionDerivedTypes<DerivedPoint3D, DerivedPoint2D, DerivedJ3D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint3D>::Type p3d_eval(p3d);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
const Scalar& k1 = param_[4];
|
||||
const Scalar& k2 = param_[5];
|
||||
const Scalar& k3 = param_[6];
|
||||
const Scalar& k4 = param_[7];
|
||||
|
||||
const Scalar& x = p3d_eval[0];
|
||||
const Scalar& y = p3d_eval[1];
|
||||
const Scalar& z = p3d_eval[2];
|
||||
|
||||
const Scalar r2 = x * x + y * y;
|
||||
const Scalar r = sqrt(r2);
|
||||
|
||||
bool is_valid = true;
|
||||
if (r > Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
const Scalar theta = atan2(r, z);
|
||||
const Scalar theta2 = theta * theta;
|
||||
|
||||
Scalar r_theta = k4 * theta2;
|
||||
r_theta += k3;
|
||||
r_theta *= theta2;
|
||||
r_theta += k2;
|
||||
r_theta *= theta2;
|
||||
r_theta += k1;
|
||||
r_theta *= theta2;
|
||||
r_theta += 1;
|
||||
r_theta *= theta;
|
||||
|
||||
const Scalar mx = x * r_theta / r;
|
||||
const Scalar my = y * r_theta / r;
|
||||
|
||||
proj[0] = fx * mx + cx;
|
||||
proj[1] = fy * my + cy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
const Scalar d_r_d_x = x / r;
|
||||
const Scalar d_r_d_y = y / r;
|
||||
|
||||
const Scalar tmp = (z * z + r2);
|
||||
const Scalar d_theta_d_x = d_r_d_x * z / tmp;
|
||||
const Scalar d_theta_d_y = d_r_d_y * z / tmp;
|
||||
const Scalar d_theta_d_z = -r / tmp;
|
||||
|
||||
Scalar d_r_theta_d_theta = Scalar(9) * k4 * theta2;
|
||||
d_r_theta_d_theta += Scalar(7) * k3;
|
||||
d_r_theta_d_theta *= theta2;
|
||||
d_r_theta_d_theta += Scalar(5) * k2;
|
||||
d_r_theta_d_theta *= theta2;
|
||||
d_r_theta_d_theta += Scalar(3) * k1;
|
||||
d_r_theta_d_theta *= theta2;
|
||||
d_r_theta_d_theta += Scalar(1);
|
||||
|
||||
d_proj_d_p3d->setZero();
|
||||
|
||||
(*d_proj_d_p3d)(0, 0) =
|
||||
fx *
|
||||
(r_theta * r + x * r * d_r_theta_d_theta * d_theta_d_x -
|
||||
x * x * r_theta / r) /
|
||||
r2;
|
||||
(*d_proj_d_p3d)(1, 0) =
|
||||
fy * y * (d_r_theta_d_theta * d_theta_d_x * r - x * r_theta / r) /
|
||||
r2;
|
||||
|
||||
(*d_proj_d_p3d)(0, 1) =
|
||||
fx * x * (d_r_theta_d_theta * d_theta_d_y * r - y * r_theta / r) /
|
||||
r2;
|
||||
|
||||
(*d_proj_d_p3d)(1, 1) =
|
||||
fy *
|
||||
(r_theta * r + y * r * d_r_theta_d_theta * d_theta_d_y -
|
||||
y * y * r_theta / r) /
|
||||
r2;
|
||||
|
||||
(*d_proj_d_p3d)(0, 2) = fx * x * d_r_theta_d_theta * d_theta_d_z / r;
|
||||
(*d_proj_d_p3d)(1, 2) = fy * y * d_r_theta_d_theta * d_theta_d_z / r;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
(*d_proj_d_param).setZero();
|
||||
(*d_proj_d_param)(0, 0) = mx;
|
||||
(*d_proj_d_param)(0, 2) = Scalar(1);
|
||||
(*d_proj_d_param)(1, 1) = my;
|
||||
(*d_proj_d_param)(1, 3) = Scalar(1);
|
||||
|
||||
(*d_proj_d_param)(0, 4) = fx * x * theta * theta2 / r;
|
||||
(*d_proj_d_param)(1, 4) = fy * y * theta * theta2 / r;
|
||||
|
||||
d_proj_d_param->col(5) = d_proj_d_param->col(4) * theta2;
|
||||
d_proj_d_param->col(6) = d_proj_d_param->col(5) * theta2;
|
||||
d_proj_d_param->col(7) = d_proj_d_param->col(6) * theta2;
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
}
|
||||
|
||||
} else {
|
||||
// Check that the point is not cloze to zero norm
|
||||
if (z < Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
is_valid = false;
|
||||
}
|
||||
|
||||
proj[0] = fx * x / z + cx;
|
||||
proj[1] = fy * y / z + cy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
|
||||
d_proj_d_p3d->setZero();
|
||||
const Scalar z2 = z * z;
|
||||
|
||||
(*d_proj_d_p3d)(0, 0) = fx / z;
|
||||
(*d_proj_d_p3d)(0, 2) = -fx * x / z2;
|
||||
|
||||
(*d_proj_d_p3d)(1, 1) = fy / z;
|
||||
(*d_proj_d_p3d)(1, 2) = -fy * y / z2;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
|
||||
d_proj_d_param->setZero();
|
||||
(*d_proj_d_param)(0, 0) = x / z;
|
||||
(*d_proj_d_param)(0, 2) = Scalar(1);
|
||||
(*d_proj_d_param)(1, 1) = y / z;
|
||||
(*d_proj_d_param)(1, 3) = Scalar(1);
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
}
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief solve for theta using Newton's method.
|
||||
///
|
||||
/// Find root of the polynomisl \f$ d(\theta) - r_{\theta} = 0 \f$ using
|
||||
/// Newton's method (https://en.wikipedia.org/wiki/Newton%27s_method). Used in
|
||||
/// \ref unproject function.
|
||||
///
|
||||
/// @param ITER number of iterations
|
||||
/// @param[in] r_theta number of iterations
|
||||
/// @param[out] d_func_d_theta derivative of the function with respect to
|
||||
/// theta at the solution
|
||||
/// @returns theta - root of the polynomial
|
||||
template <int ITER>
|
||||
inline Scalar solveTheta(const Scalar& r_theta,
|
||||
Scalar& d_func_d_theta) const {
|
||||
const Scalar& k1 = param_[4];
|
||||
const Scalar& k2 = param_[5];
|
||||
const Scalar& k3 = param_[6];
|
||||
const Scalar& k4 = param_[7];
|
||||
|
||||
Scalar theta = r_theta;
|
||||
for (int i = ITER; i > 0; i--) {
|
||||
Scalar theta2 = theta * theta;
|
||||
|
||||
Scalar func = k4 * theta2;
|
||||
func += k3;
|
||||
func *= theta2;
|
||||
func += k2;
|
||||
func *= theta2;
|
||||
func += k1;
|
||||
func *= theta2;
|
||||
func += Scalar(1);
|
||||
func *= theta;
|
||||
|
||||
d_func_d_theta = Scalar(9) * k4 * theta2;
|
||||
d_func_d_theta += Scalar(7) * k3;
|
||||
d_func_d_theta *= theta2;
|
||||
d_func_d_theta += Scalar(5) * k2;
|
||||
d_func_d_theta *= theta2;
|
||||
d_func_d_theta += Scalar(3) * k1;
|
||||
d_func_d_theta *= theta2;
|
||||
d_func_d_theta += Scalar(1);
|
||||
|
||||
// Iteration of Newton method
|
||||
theta += (r_theta - func) / d_func_d_theta;
|
||||
}
|
||||
|
||||
return theta;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobians
|
||||
///
|
||||
/// The unprojection function is computed as follows: \f{align}{
|
||||
/// \pi^{-1}(\mathbf{u}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// \sin(\theta^{*}) ~ \frac{m_x}{r_{\theta}}
|
||||
/// \\ \sin(\theta^{*}) ~ \frac{m_y}{r_{\theta}}
|
||||
/// \\ \cos(\theta^{*})
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ m_x &= \frac{u - c_x}{f_x},
|
||||
/// \\ m_y &= \frac{v - c_y}{f_y},
|
||||
/// \\ r_{\theta} &= \sqrt{m_x^2 + m_y^2},
|
||||
/// \\ \theta^{*} &= d^{-1}(r_{\theta}).
|
||||
/// \f}
|
||||
///
|
||||
/// \f$\theta^{*}\f$ is the solution of \f$d(\theta)=r_{\theta}\f$. See \ref
|
||||
/// solve_theta for details.
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @param[out] d_p3d_d_param point if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to intrinsic parameters
|
||||
/// @return if unprojection is valid
|
||||
template <class DerivedPoint2D, class DerivedPoint3D,
|
||||
class DerivedJ2D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool unproject(const Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
DerivedJ2D d_p3d_d_proj = nullptr,
|
||||
DerivedJparam d_p3d_d_param = nullptr) const {
|
||||
checkUnprojectionDerivedTypes<DerivedPoint2D, DerivedPoint3D, DerivedJ2D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint2D>::Type proj_eval(proj);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
|
||||
const Scalar mx = (proj_eval[0] - cx) / fx;
|
||||
const Scalar my = (proj_eval[1] - cy) / fy;
|
||||
|
||||
Scalar theta(0);
|
||||
Scalar sin_theta(0);
|
||||
Scalar cos_theta(1);
|
||||
Scalar thetad = sqrt(mx * mx + my * my);
|
||||
Scalar scaling(1);
|
||||
Scalar d_func_d_theta(0);
|
||||
|
||||
if (thetad > Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
theta = solveTheta<3>(thetad, d_func_d_theta);
|
||||
|
||||
sin_theta = sin(theta);
|
||||
cos_theta = cos(theta);
|
||||
scaling = sin_theta / thetad;
|
||||
}
|
||||
|
||||
p3d.setZero();
|
||||
p3d[0] = mx * scaling;
|
||||
p3d[1] = my * scaling;
|
||||
p3d[2] = cos_theta;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t> ||
|
||||
!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
Scalar d_thetad_d_mx = Scalar(0);
|
||||
Scalar d_thetad_d_my = Scalar(0);
|
||||
Scalar d_scaling_d_thetad = Scalar(0);
|
||||
Scalar d_cos_d_thetad = Scalar(0);
|
||||
|
||||
Scalar d_scaling_d_k1 = Scalar(0);
|
||||
Scalar d_cos_d_k1 = Scalar(0);
|
||||
|
||||
Scalar theta2 = Scalar(0);
|
||||
|
||||
if (thetad > Sophus::Constants<Scalar>::epsilonSqrt()) {
|
||||
d_thetad_d_mx = mx / thetad;
|
||||
d_thetad_d_my = my / thetad;
|
||||
|
||||
theta2 = theta * theta;
|
||||
|
||||
d_scaling_d_thetad = (thetad * cos_theta / d_func_d_theta - sin_theta) /
|
||||
(thetad * thetad);
|
||||
|
||||
d_cos_d_thetad = sin_theta / d_func_d_theta;
|
||||
|
||||
d_scaling_d_k1 =
|
||||
-cos_theta * theta * theta2 / (d_func_d_theta * thetad);
|
||||
|
||||
d_cos_d_k1 = d_cos_d_thetad * theta * theta2;
|
||||
}
|
||||
|
||||
const Scalar d_res0_d_mx =
|
||||
scaling + mx * d_scaling_d_thetad * d_thetad_d_mx;
|
||||
const Scalar d_res0_d_my = mx * d_scaling_d_thetad * d_thetad_d_my;
|
||||
|
||||
const Scalar d_res1_d_mx = my * d_scaling_d_thetad * d_thetad_d_mx;
|
||||
const Scalar d_res1_d_my =
|
||||
scaling + my * d_scaling_d_thetad * d_thetad_d_my;
|
||||
|
||||
const Scalar d_res2_d_mx = -d_cos_d_thetad * d_thetad_d_mx;
|
||||
const Scalar d_res2_d_my = -d_cos_d_thetad * d_thetad_d_my;
|
||||
|
||||
constexpr int SIZE_3D = DerivedPoint3D::SizeAtCompileTime;
|
||||
Eigen::Matrix<Scalar, SIZE_3D, 1> c0, c1;
|
||||
|
||||
c0.setZero();
|
||||
c0(0) = d_res0_d_mx / fx;
|
||||
c0(1) = d_res1_d_mx / fx;
|
||||
c0(2) = d_res2_d_mx / fx;
|
||||
|
||||
c1.setZero();
|
||||
c1(0) = d_res0_d_my / fy;
|
||||
c1(1) = d_res1_d_my / fy;
|
||||
c1(2) = d_res2_d_my / fy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_proj);
|
||||
d_p3d_d_proj->col(0) = c0;
|
||||
d_p3d_d_proj->col(1) = c1;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_param);
|
||||
d_p3d_d_param->setZero();
|
||||
|
||||
d_p3d_d_param->col(2) = -c0;
|
||||
d_p3d_d_param->col(3) = -c1;
|
||||
|
||||
d_p3d_d_param->col(0) = -c0 * mx;
|
||||
d_p3d_d_param->col(1) = -c1 * my;
|
||||
|
||||
(*d_p3d_d_param)(0, 4) = mx * d_scaling_d_k1;
|
||||
(*d_p3d_d_param)(1, 4) = my * d_scaling_d_k1;
|
||||
(*d_p3d_d_param)(2, 4) = d_cos_d_k1;
|
||||
|
||||
d_p3d_d_param->col(5) = d_p3d_d_param->col(4) * theta2;
|
||||
d_p3d_d_param->col(6) = d_p3d_d_param->col(5) * theta2;
|
||||
d_p3d_d_param->col(7) = d_p3d_d_param->col(6) * theta2;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_param);
|
||||
UNUSED(d_scaling_d_k1);
|
||||
UNUSED(d_cos_d_k1);
|
||||
}
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc
|
||||
///
|
||||
/// @param[in] inc increment vector
|
||||
void operator+=(const VecN& inc) { param_ += inc; }
|
||||
|
||||
/// @brief Returns a const reference to the intrinsic parameters vector
|
||||
///
|
||||
/// The order is following: \f$ \left[f_x, f_y, c_x, c_y, k_1, k_2, k_3, k_4
|
||||
/// \right]^T \f$
|
||||
/// @return const reference to the intrinsic parameters vector
|
||||
const VecN& getParam() const { return param_; }
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// Initializes the camera model to \f$ \left[f_x, f_y, c_x, c_y, 0, 0, 0, 0
|
||||
/// \right]^T \f$
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
param_[0] = init[0];
|
||||
param_[1] = init[1];
|
||||
param_[2] = init[2];
|
||||
param_[3] = init[3];
|
||||
param_[4] = 0;
|
||||
param_[5] = 0;
|
||||
param_[6] = 0;
|
||||
param_[7] = 0;
|
||||
}
|
||||
|
||||
/// @brief Projections used for unit-tests
|
||||
static Eigen::aligned_vector<KannalaBrandtCamera4> getTestProjections() {
|
||||
Eigen::aligned_vector<KannalaBrandtCamera4> res;
|
||||
|
||||
VecN vec1;
|
||||
vec1 << 379.045, 379.008, 505.512, 509.969, 0.00693023, -0.0013828,
|
||||
-0.000272596, -0.000452646;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
VecN param_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
324
thirdparty/basalt-headers/include/basalt/camera/pinhole_camera.hpp
vendored
Normal file
324
thirdparty/basalt-headers/include/basalt/camera/pinhole_camera.hpp
vendored
Normal file
@@ -0,0 +1,324 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of pinhole camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/camera_static_assert.hpp>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Pinhole camera model
|
||||
///
|
||||
/// This model has N=4 parameters \f$ \mathbf{i} = \left[f_x, f_y, c_x, c_y
|
||||
/// \right]^T \f$ with. See \ref
|
||||
/// project and \ref unproject functions for more details.
|
||||
template <typename Scalar_ = double>
|
||||
class PinholeCamera {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int N = 4; ///< Number of intrinsic parameters.
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using VecN = Eigen::Matrix<Scalar, N, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat2N = Eigen::Matrix<Scalar, 2, N>;
|
||||
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4N = Eigen::Matrix<Scalar, 4, N>;
|
||||
|
||||
/// @brief Default constructor with zero intrinsics
|
||||
PinholeCamera() { param_.setZero(); }
|
||||
|
||||
/// @brief Construct camera model with given vector of intrinsics
|
||||
///
|
||||
/// @param[in] p vector of intrinsic parameters [fx, fy, cx, cy]
|
||||
explicit PinholeCamera(const VecN& p) { param_ = p; }
|
||||
|
||||
/// @brief Cast to different scalar type
|
||||
template <class Scalar2>
|
||||
PinholeCamera<Scalar2> cast() const {
|
||||
return PinholeCamera<Scalar2>(param_.template cast<Scalar2>());
|
||||
}
|
||||
|
||||
/// @brief Camera model name
|
||||
///
|
||||
/// @return "pinhole"
|
||||
static std::string getName() { return "pinhole"; }
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobians
|
||||
///
|
||||
/// Projection function is defined as follows:
|
||||
/// \f{align}{
|
||||
/// \pi(\mathbf{x}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// f_x{\frac{x}{z}}
|
||||
/// \\ f_y{\frac{y}{z}}
|
||||
/// \\ \end{bmatrix}
|
||||
/// +
|
||||
/// \begin{bmatrix}
|
||||
/// c_x
|
||||
/// \\ c_y
|
||||
/// \\ \end{bmatrix}.
|
||||
/// \f}
|
||||
/// A set of 3D points that results in valid projection is expressed as
|
||||
/// follows: \f{align}{
|
||||
/// \Omega &= \{\mathbf{x} \in \mathbb{R}^3 ~|~ z > 0 \}
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @param[out] d_proj_d_param point if not nullptr computed Jacobian of
|
||||
/// projection with respect to intrinsic parameters
|
||||
/// @return if projection is valid
|
||||
template <class DerivedPoint3D, class DerivedPoint2D,
|
||||
class DerivedJ3D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool project(const Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
DerivedJ3D d_proj_d_p3d = nullptr,
|
||||
DerivedJparam d_proj_d_param = nullptr) const {
|
||||
checkProjectionDerivedTypes<DerivedPoint3D, DerivedPoint2D, DerivedJ3D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint3D>::Type p3d_eval(p3d);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
|
||||
const Scalar& x = p3d_eval[0];
|
||||
const Scalar& y = p3d_eval[1];
|
||||
const Scalar& z = p3d_eval[2];
|
||||
|
||||
proj[0] = fx * x / z + cx;
|
||||
proj[1] = fy * y / z + cy;
|
||||
|
||||
const bool is_valid = z >= Sophus::Constants<Scalar>::epsilonSqrt();
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
|
||||
d_proj_d_p3d->setZero();
|
||||
const Scalar z2 = z * z;
|
||||
|
||||
(*d_proj_d_p3d)(0, 0) = fx / z;
|
||||
(*d_proj_d_p3d)(0, 2) = -fx * x / z2;
|
||||
|
||||
(*d_proj_d_p3d)(1, 1) = fy / z;
|
||||
(*d_proj_d_p3d)(1, 2) = -fy * y / z2;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
d_proj_d_param->setZero();
|
||||
(*d_proj_d_param)(0, 0) = x / z;
|
||||
(*d_proj_d_param)(0, 2) = Scalar(1);
|
||||
(*d_proj_d_param)(1, 1) = y / z;
|
||||
(*d_proj_d_param)(1, 3) = Scalar(1);
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobians
|
||||
///
|
||||
/// The unprojection function is computed as follows: \f{align}{
|
||||
/// \pi^{-1}(\mathbf{u}, \mathbf{i}) &=
|
||||
/// \frac{1}{m_x^2 + m_y^2 + 1}
|
||||
/// \begin{bmatrix}
|
||||
/// m_x \\ m_y \\ 1
|
||||
/// \\ \end{bmatrix}
|
||||
/// \\ m_x &= \frac{u - c_x}{f_x},
|
||||
/// \\ m_y &= \frac{v - c_y}{f_y}.
|
||||
/// \f}
|
||||
///
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to proj
|
||||
/// @param[out] d_p3d_d_param point if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to intrinsic parameters
|
||||
/// @return if unprojection is valid
|
||||
template <class DerivedPoint2D, class DerivedPoint3D,
|
||||
class DerivedJ2D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool unproject(const Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
DerivedJ2D d_p3d_d_proj = nullptr,
|
||||
DerivedJparam d_p3d_d_param = nullptr) const {
|
||||
checkUnprojectionDerivedTypes<DerivedPoint2D, DerivedPoint3D, DerivedJ2D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint2D>::Type proj_eval(proj);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
|
||||
const Scalar mx = (proj_eval[0] - cx) / fx;
|
||||
const Scalar my = (proj_eval[1] - cy) / fy;
|
||||
|
||||
const Scalar r2 = mx * mx + my * my;
|
||||
|
||||
const Scalar norm = sqrt(Scalar(1) + r2);
|
||||
const Scalar norm_inv = Scalar(1) / norm;
|
||||
|
||||
p3d.setZero();
|
||||
p3d[0] = mx * norm_inv;
|
||||
p3d[1] = my * norm_inv;
|
||||
p3d[2] = norm_inv;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t> ||
|
||||
!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
const Scalar d_norm_inv_d_r2 =
|
||||
-Scalar(0.5) * norm_inv * norm_inv * norm_inv;
|
||||
|
||||
constexpr int SIZE_3D = DerivedPoint3D::SizeAtCompileTime;
|
||||
Eigen::Matrix<Scalar, SIZE_3D, 1> c0, c1;
|
||||
|
||||
c0.setZero();
|
||||
c0(0) = (norm_inv + 2 * mx * mx * d_norm_inv_d_r2) / fx;
|
||||
c0(1) = (2 * my * mx * d_norm_inv_d_r2) / fx;
|
||||
c0(2) = 2 * mx * d_norm_inv_d_r2 / fx;
|
||||
|
||||
c1.setZero();
|
||||
c1(0) = (2 * my * mx * d_norm_inv_d_r2) / fy;
|
||||
c1(1) = (norm_inv + 2 * my * my * d_norm_inv_d_r2) / fy;
|
||||
c1(2) = 2 * my * d_norm_inv_d_r2 / fy;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_proj);
|
||||
d_p3d_d_proj->col(0) = c0;
|
||||
d_p3d_d_proj->col(1) = c1;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_param);
|
||||
d_p3d_d_param->col(2) = -c0;
|
||||
d_p3d_d_param->col(3) = -c1;
|
||||
|
||||
d_p3d_d_param->col(0) = -c0 * mx;
|
||||
d_p3d_d_param->col(1) = -c1 * my;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// Initializes the camera model to \f$ \left[f_x, f_y, c_x, c_y, \right]^T
|
||||
/// \f$
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
param_[0] = init[0];
|
||||
param_[1] = init[1];
|
||||
param_[2] = init[2];
|
||||
param_[3] = init[3];
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc
|
||||
///
|
||||
/// @param[in] inc increment vector
|
||||
void operator+=(const VecN& inc) { param_ += inc; }
|
||||
|
||||
/// @brief Returns a const reference to the intrinsic parameters vector
|
||||
///
|
||||
/// The order is following: \f$ \left[f_x, f_y, c_x, c_y, \right]^T \f$
|
||||
/// @return const reference to the intrinsic parameters vector
|
||||
const VecN& getParam() const { return param_; }
|
||||
|
||||
/// @brief Projections used for unit-tests
|
||||
static Eigen::aligned_vector<PinholeCamera> getTestProjections() {
|
||||
Eigen::aligned_vector<PinholeCamera> res;
|
||||
|
||||
VecN vec1;
|
||||
|
||||
// Euroc
|
||||
vec1 << 460.76484651566468, 459.4051018049483, 365.8937161309615,
|
||||
249.33499869752445;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
// TUM VI 512
|
||||
vec1 << 191.14799816648748, 191.13150946585135, 254.95857715233118,
|
||||
256.8815466235898;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Resolutions used for unit-tests
|
||||
static Eigen::aligned_vector<Eigen::Vector2i> getTestResolutions() {
|
||||
Eigen::aligned_vector<Eigen::Vector2i> res;
|
||||
|
||||
res.emplace_back(752, 480);
|
||||
res.emplace_back(512, 512);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
VecN param_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
155
thirdparty/basalt-headers/include/basalt/camera/stereographic_param.hpp
vendored
Normal file
155
thirdparty/basalt-headers/include/basalt/camera/stereographic_param.hpp
vendored
Normal file
@@ -0,0 +1,155 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of stereographic parametrization for unit vectors
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
/// @brief Stereographic projection for minimal representation of unit vectors
|
||||
///
|
||||
/// \image html stereographic.png
|
||||
/// The projection is defined on the entire
|
||||
/// sphere, except at one point: the projection point. Where it is defined, the
|
||||
/// mapping is smooth and bijective. The illustrations shows 3 examples of unit
|
||||
/// vectors parametrized with a point in the 2D plane. See
|
||||
/// \ref project and \ref unproject functions for more details.
|
||||
template <typename Scalar_ = double>
|
||||
class StereographicParam {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobian
|
||||
///
|
||||
/// Projection function is defined as follows:
|
||||
/// \f{align}{
|
||||
/// \pi(\mathbf{x}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// {\frac{x}{d + z}}
|
||||
/// \\ {\frac{y}{d + z}}
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ d &= \sqrt{x^2 + y^2 + z^2}.
|
||||
/// \f}
|
||||
/// @param[in] p3d point to project [x,y,z]
|
||||
/// @param[out] d_r_d_p if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @return 2D projection of the point which parametrizes the corresponding
|
||||
/// direction vector
|
||||
static inline Vec2 project(const Vec4& p3d, Mat24* d_r_d_p = nullptr) {
|
||||
const Scalar sqrt = p3d.template head<3>().norm();
|
||||
const Scalar norm = p3d[2] + sqrt;
|
||||
const Scalar norm_inv = Scalar(1) / norm;
|
||||
|
||||
Vec2 res(p3d[0] * norm_inv, p3d[1] * norm_inv);
|
||||
|
||||
if (d_r_d_p) {
|
||||
Scalar norm_inv2 = norm_inv * norm_inv;
|
||||
Scalar tmp = -norm_inv2 / sqrt;
|
||||
|
||||
(*d_r_d_p).setZero();
|
||||
|
||||
(*d_r_d_p)(0, 0) = norm_inv + p3d[0] * p3d[0] * tmp;
|
||||
(*d_r_d_p)(1, 0) = p3d[0] * p3d[1] * tmp;
|
||||
|
||||
(*d_r_d_p)(1, 1) = norm_inv + p3d[1] * p3d[1] * tmp;
|
||||
(*d_r_d_p)(0, 1) = p3d[0] * p3d[1] * tmp;
|
||||
|
||||
(*d_r_d_p)(0, 2) = p3d[0] * norm * tmp;
|
||||
(*d_r_d_p)(1, 2) = p3d[1] * norm * tmp;
|
||||
|
||||
(*d_r_d_p)(0, 3) = Scalar(0);
|
||||
(*d_r_d_p)(1, 3) = Scalar(0);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobian
|
||||
///
|
||||
/// The unprojection function is computed as follows: \f{align}{
|
||||
/// \pi^{-1}(\mathbf{u}) &=
|
||||
/// \frac{2}{1 + x^2 + y^2}
|
||||
/// \begin{bmatrix}
|
||||
/// u \\ v \\ 1
|
||||
/// \\ \end{bmatrix}-\begin{bmatrix}
|
||||
/// 0 \\ 0 \\ 1
|
||||
/// \\ \end{bmatrix}.
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] proj point to unproject [u,v]
|
||||
/// @param[out] d_r_d_p if not nullptr computed Jacobian of unprojection
|
||||
/// with respect to proj
|
||||
/// @return unprojected 3D unit vector
|
||||
static inline Vec4 unproject(const Vec2& proj, Mat42* d_r_d_p = nullptr) {
|
||||
const Scalar x2 = proj[0] * proj[0];
|
||||
const Scalar y2 = proj[1] * proj[1];
|
||||
const Scalar r2 = x2 + y2;
|
||||
|
||||
const Scalar norm_inv = Scalar(2) / (Scalar(1) + r2);
|
||||
|
||||
Vec4 res(proj[0] * norm_inv, proj[1] * norm_inv, norm_inv - Scalar(1),
|
||||
Scalar(0));
|
||||
|
||||
if (d_r_d_p) {
|
||||
const Scalar norm_inv2 = norm_inv * norm_inv;
|
||||
const Scalar xy = proj[0] * proj[1];
|
||||
|
||||
(*d_r_d_p)(0, 0) = (norm_inv - x2 * norm_inv2);
|
||||
(*d_r_d_p)(0, 1) = -xy * norm_inv2;
|
||||
|
||||
(*d_r_d_p)(1, 0) = -xy * norm_inv2;
|
||||
(*d_r_d_p)(1, 1) = (norm_inv - y2 * norm_inv2);
|
||||
|
||||
(*d_r_d_p)(2, 0) = -proj[0] * norm_inv2;
|
||||
(*d_r_d_p)(2, 1) = -proj[1] * norm_inv2;
|
||||
|
||||
(*d_r_d_p)(3, 0) = Scalar(0);
|
||||
(*d_r_d_p)(3, 1) = Scalar(0);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
408
thirdparty/basalt-headers/include/basalt/camera/unified_camera.hpp
vendored
Normal file
408
thirdparty/basalt-headers/include/basalt/camera/unified_camera.hpp
vendored
Normal file
@@ -0,0 +1,408 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
@file
|
||||
@brief Implementation of unified camera model
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <basalt/camera/camera_static_assert.hpp>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
/// @brief Unified camera model
|
||||
///
|
||||
/// \image html ucm.png
|
||||
/// This model has N=5 parameters \f$ \mathbf{i} = \left[f_x, f_y, c_x, c_y,
|
||||
/// \alpha \right]^T \f$ with \f$ \alpha \in [0,1] \f$.
|
||||
/// See \ref project and \ref unproject functions for more details.
|
||||
template <typename Scalar_ = double>
|
||||
class UnifiedCamera {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int N = 5; ///< Number of intrinsic parameters.
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
|
||||
using VecN = Eigen::Matrix<Scalar, N, 1>;
|
||||
|
||||
using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
|
||||
using Mat2N = Eigen::Matrix<Scalar, 2, N>;
|
||||
|
||||
using Mat42 = Eigen::Matrix<Scalar, 4, 2>;
|
||||
using Mat4N = Eigen::Matrix<Scalar, 4, N>;
|
||||
|
||||
/// @brief Default constructor with zero intrinsics
|
||||
UnifiedCamera() { param_.setZero(); }
|
||||
|
||||
/// @brief Construct camera model with given vector of intrinsics
|
||||
///
|
||||
/// @param[in] p vector of intrinsic parameters [fx, fy, cx, cy, alpha]
|
||||
explicit UnifiedCamera(const VecN& p) { param_ = p; }
|
||||
|
||||
/// @brief Cast to different scalar type
|
||||
template <class Scalar2>
|
||||
UnifiedCamera<Scalar2> cast() const {
|
||||
return UnifiedCamera<Scalar2>(param_.template cast<Scalar2>());
|
||||
}
|
||||
|
||||
/// @brief Camera model name
|
||||
///
|
||||
/// @return "ucm"
|
||||
static std::string getName() { return "ucm"; }
|
||||
|
||||
/// @brief Project the point and optionally compute Jacobians
|
||||
///
|
||||
/// Projection function is defined as follows:
|
||||
/// \f{align}{
|
||||
/// \pi(\mathbf{x}, \mathbf{i}) &=
|
||||
/// \begin{bmatrix}
|
||||
/// f_x{\frac{x}{\alpha d + (1-\alpha) z}}
|
||||
/// \\ f_y{\frac{y}{\alpha d + (1-\alpha) z}}
|
||||
/// \\ \end{bmatrix}
|
||||
/// +
|
||||
/// \begin{bmatrix}
|
||||
/// c_x
|
||||
/// \\ c_y
|
||||
/// \\ \end{bmatrix},
|
||||
/// \\ d &= \sqrt{x^2 + y^2 + z^2}.
|
||||
/// \f}
|
||||
/// A set of 3D points that results in valid projection is expressed as
|
||||
/// follows: \f{align}{
|
||||
/// \Omega &= \{\mathbf{x} \in \mathbb{R}^3 ~|~ z > -wd \},
|
||||
/// \\ w &= \begin{cases} \frac{\alpha}{1-\alpha}, & \mbox{if } \alpha \le
|
||||
/// 0.5,
|
||||
/// \\ \frac{1-\alpha}{\alpha} & \mbox{if } \alpha > 0.5, \end{cases} \f}
|
||||
///
|
||||
/// @param[in] p3d point to project
|
||||
/// @param[out] proj result of projection
|
||||
/// @param[out] d_proj_d_p3d if not nullptr computed Jacobian of projection
|
||||
/// with respect to p3d
|
||||
/// @param[out] d_proj_d_param point if not nullptr computed Jacobian of
|
||||
/// projection with respect to intrinsic parameters
|
||||
/// @return if projection is valid
|
||||
template <class DerivedPoint3D, class DerivedPoint2D,
|
||||
class DerivedJ3D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool project(const Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
DerivedJ3D d_proj_d_p3d = nullptr,
|
||||
DerivedJparam d_proj_d_param = nullptr) const {
|
||||
const typename EvalOrReference<DerivedPoint3D>::Type p3d_eval(p3d);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
const Scalar& alpha = param_[4];
|
||||
|
||||
const Scalar& x = p3d_eval[0];
|
||||
const Scalar& y = p3d_eval[1];
|
||||
const Scalar& z = p3d_eval[2];
|
||||
|
||||
const Scalar r2 = x * x + y * y;
|
||||
const Scalar rho2 = r2 + z * z;
|
||||
const Scalar rho = sqrt(rho2);
|
||||
|
||||
const Scalar norm = alpha * rho + (Scalar(1) - alpha) * z;
|
||||
|
||||
const Scalar mx = x / norm;
|
||||
const Scalar my = y / norm;
|
||||
|
||||
proj = Vec2(fx * mx + cx, fy * my + cy);
|
||||
|
||||
// Check if valid
|
||||
const Scalar w = alpha > Scalar(0.5) ? (Scalar(1) - alpha) / alpha
|
||||
: alpha / (Scalar(1) - alpha);
|
||||
const bool is_valid = (z > -w * rho);
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ3D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_p3d);
|
||||
const Scalar denom = norm * norm * rho;
|
||||
const Scalar mid = -(alpha * x * y);
|
||||
const Scalar add = norm * rho;
|
||||
const Scalar addz = (alpha * z + (Scalar(1) - alpha) * rho);
|
||||
|
||||
d_proj_d_p3d->setZero();
|
||||
(*d_proj_d_p3d)(0, 0) = fx * (add - x * x * alpha);
|
||||
(*d_proj_d_p3d)(1, 0) = fy * mid;
|
||||
(*d_proj_d_p3d)(0, 1) = fx * mid;
|
||||
(*d_proj_d_p3d)(1, 1) = fy * (add - y * y * alpha);
|
||||
(*d_proj_d_p3d)(0, 2) = -fx * x * addz;
|
||||
(*d_proj_d_p3d)(1, 2) = -fy * y * addz;
|
||||
|
||||
(*d_proj_d_p3d) /= denom;
|
||||
} else {
|
||||
UNUSED(d_proj_d_p3d);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_proj_d_param);
|
||||
const Scalar norm2 = norm * norm;
|
||||
|
||||
(*d_proj_d_param).setZero();
|
||||
(*d_proj_d_param)(0, 0) = mx;
|
||||
(*d_proj_d_param)(0, 2) = Scalar(1);
|
||||
(*d_proj_d_param)(1, 1) = my;
|
||||
(*d_proj_d_param)(1, 3) = Scalar(1);
|
||||
|
||||
const Scalar tmp_x = -fx * x / norm2;
|
||||
const Scalar tmp_y = -fy * y / norm2;
|
||||
|
||||
const Scalar tmp4 = (rho - z);
|
||||
|
||||
(*d_proj_d_param)(0, 4) = tmp_x * tmp4;
|
||||
(*d_proj_d_param)(1, 4) = tmp_y * tmp4;
|
||||
} else {
|
||||
UNUSED(d_proj_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Unproject the point and optionally compute Jacobians
|
||||
///
|
||||
/// The unprojection function is computed as follows: \f{align}{
|
||||
/// \pi ^ { -1 }(\mathbf{u}, \mathbf{i}) &=
|
||||
/// \frac{\xi + \sqrt{1 + (1 - \xi ^ 2) r ^ 2}} {
|
||||
/// 1 + r ^ 2
|
||||
/// }
|
||||
/// \begin{bmatrix} m_x \\ m_y \\ 1 \\ \end{bmatrix} -
|
||||
/// \begin {bmatrix} 0 \\ 0 \\ \xi \\ \end{bmatrix},
|
||||
/// \\ m_x &= \frac{u - c_x}{f_x}(1-\alpha),
|
||||
/// \\ m_y &= \frac{v - c_y}{f_y}(1-\alpha),
|
||||
/// \\ r^2 &= m_x^2 + m_y^2,
|
||||
/// \\ \xi &= \frac{\alpha}{1-\alpha}.
|
||||
/// \f}
|
||||
///
|
||||
/// The valid range of unprojections is \f{align}{
|
||||
/// \Theta &=
|
||||
/// \begin{cases}
|
||||
/// \mathbb{R}^2 & \mbox{if } \alpha \le 0.5
|
||||
/// \\ \{ \mathbf{u} \in \mathbb{R}^2 ~|~ r^2 \le
|
||||
/// \frac{(1-\alpha)^2}{2\alpha
|
||||
/// - 1} \} & \mbox{if } \alpha > 0.5 \end{cases}
|
||||
/// \f}
|
||||
///
|
||||
/// @param[in] proj point to unproject
|
||||
/// @param[out] p3d result of unprojection
|
||||
/// @param[out] d_p3d_d_proj if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to proj
|
||||
/// @param[out] d_p3d_d_param point if not nullptr computed Jacobian of
|
||||
/// unprojection with respect to intrinsic parameters
|
||||
/// @return if unprojection is valid
|
||||
template <class DerivedPoint2D, class DerivedPoint3D,
|
||||
class DerivedJ2D = std::nullptr_t,
|
||||
class DerivedJparam = std::nullptr_t>
|
||||
inline bool unproject(const Eigen::MatrixBase<DerivedPoint2D>& proj,
|
||||
Eigen::MatrixBase<DerivedPoint3D>& p3d,
|
||||
DerivedJ2D d_p3d_d_proj = nullptr,
|
||||
DerivedJparam d_p3d_d_param = nullptr) const {
|
||||
checkUnprojectionDerivedTypes<DerivedPoint2D, DerivedPoint3D, DerivedJ2D,
|
||||
DerivedJparam, N>();
|
||||
|
||||
const typename EvalOrReference<DerivedPoint2D>::Type proj_eval(proj);
|
||||
|
||||
const Scalar& fx = param_[0];
|
||||
const Scalar& fy = param_[1];
|
||||
const Scalar& cx = param_[2];
|
||||
const Scalar& cy = param_[3];
|
||||
const Scalar& alpha = param_[4];
|
||||
|
||||
const Scalar& u = proj_eval[0];
|
||||
const Scalar& v = proj_eval[1];
|
||||
|
||||
const Scalar xi = alpha / (Scalar(1) - alpha);
|
||||
|
||||
const Scalar mxx = (u - cx) / fx;
|
||||
const Scalar myy = (v - cy) / fy;
|
||||
|
||||
const Scalar mx = (Scalar(1) - alpha) * mxx;
|
||||
const Scalar my = (Scalar(1) - alpha) * myy;
|
||||
|
||||
const Scalar r2 = mx * mx + my * my;
|
||||
|
||||
// Check if valid
|
||||
const bool is_valid = !static_cast<bool>(
|
||||
(alpha > Scalar(0.5)) &&
|
||||
(r2 >= Scalar(1) / ((Scalar(2) * alpha - Scalar(1)))));
|
||||
|
||||
const Scalar xi2 = xi * xi;
|
||||
|
||||
const Scalar n = sqrt(Scalar(1) + (Scalar(1) - xi2) * (r2));
|
||||
const Scalar m = (Scalar(1) + r2);
|
||||
|
||||
const Scalar k = (xi + n) / m;
|
||||
|
||||
p3d.setZero();
|
||||
p3d[0] = k * mx;
|
||||
p3d[1] = k * my;
|
||||
p3d[2] = k - xi;
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t> ||
|
||||
!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
const Scalar dk_dmx = -Scalar(2) * mx * (n + xi) / (m * m) +
|
||||
mx * (Scalar(1) - xi2) / (n * m);
|
||||
const Scalar dk_dmy = -Scalar(2) * my * (n + xi) / (m * m) +
|
||||
my * (Scalar(1) - xi2) / (n * m);
|
||||
|
||||
constexpr int SIZE_3D = DerivedPoint3D::SizeAtCompileTime;
|
||||
Eigen::Matrix<Scalar, SIZE_3D, 1> c0, c1;
|
||||
|
||||
c0.setZero();
|
||||
c0(0) = (dk_dmx * mx + k) / fx;
|
||||
c0(1) = dk_dmx * my / fx;
|
||||
c0(2) = dk_dmx / fx;
|
||||
|
||||
c1.setZero();
|
||||
c1(0) = dk_dmy * mx / fy;
|
||||
c1(1) = (dk_dmy * my + k) / fy;
|
||||
c1(2) = dk_dmy / fy;
|
||||
|
||||
c0 *= (1 - alpha);
|
||||
c1 *= (1 - alpha);
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJ2D, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_proj);
|
||||
d_p3d_d_proj->col(0) = c0;
|
||||
d_p3d_d_proj->col(1) = c1;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
}
|
||||
|
||||
if constexpr (!std::is_same_v<DerivedJparam, std::nullptr_t>) {
|
||||
BASALT_ASSERT(d_p3d_d_param);
|
||||
const Scalar d_xi_d_alpha =
|
||||
Scalar(1) / ((Scalar(1) - alpha) * (Scalar(1) - alpha));
|
||||
const Scalar d_m_d_alpha =
|
||||
-Scalar(2) * (Scalar(1) - alpha) * (mxx * mxx + myy * myy);
|
||||
|
||||
const Scalar d_n_d_alpha = -(mxx * mxx + myy * myy) / n;
|
||||
|
||||
const Scalar dk_d_alpha =
|
||||
((d_xi_d_alpha + d_n_d_alpha) * m - d_m_d_alpha * (xi + n)) /
|
||||
(m * m);
|
||||
|
||||
d_p3d_d_param->setZero();
|
||||
d_p3d_d_param->col(0) = -mxx * c0;
|
||||
d_p3d_d_param->col(1) = -myy * c1;
|
||||
d_p3d_d_param->col(2) = -c0;
|
||||
d_p3d_d_param->col(3) = -c1;
|
||||
|
||||
(*d_p3d_d_param)(0, 4) = dk_d_alpha * mx - k * mxx;
|
||||
(*d_p3d_d_param)(1, 4) = dk_d_alpha * my - k * myy;
|
||||
(*d_p3d_d_param)(2, 4) = dk_d_alpha - d_xi_d_alpha;
|
||||
} else {
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
} else {
|
||||
UNUSED(d_p3d_d_proj);
|
||||
UNUSED(d_p3d_d_param);
|
||||
}
|
||||
|
||||
return is_valid;
|
||||
}
|
||||
|
||||
/// @brief Set parameters from initialization
|
||||
///
|
||||
/// Initializes the camera model to \f$ \left[f_x, f_y, c_x, c_y, 0.5,
|
||||
/// \right]^T \f$
|
||||
///
|
||||
/// @param[in] init vector [fx, fy, cx, cy]
|
||||
inline void setFromInit(const Vec4& init) {
|
||||
param_[0] = init[0];
|
||||
param_[1] = init[1];
|
||||
param_[2] = init[2];
|
||||
param_[3] = init[3];
|
||||
param_[4] = 0.5;
|
||||
}
|
||||
|
||||
/// @brief Increment intrinsic parameters by inc and clamp the values to the
|
||||
/// valid range
|
||||
///
|
||||
/// @param[in] inc increment vector
|
||||
void operator+=(const VecN& inc) {
|
||||
param_ += inc;
|
||||
// alpha in [0, 1]
|
||||
param_[4] = std::clamp(param_[4], Scalar(0), Scalar(1));
|
||||
}
|
||||
|
||||
/// @brief Returns a const reference to the intrinsic parameters vector
|
||||
///
|
||||
/// The order is following: \f$ \left[f_x, f_y, c_x, c_y, \xi, \alpha
|
||||
/// \right]^T \f$
|
||||
/// @return const reference to the intrinsic parameters vector
|
||||
const VecN& getParam() const { return param_; }
|
||||
|
||||
/// @brief Projections used for unit-tests
|
||||
static Eigen::aligned_vector<UnifiedCamera> getTestProjections() {
|
||||
Eigen::aligned_vector<UnifiedCamera> res;
|
||||
|
||||
VecN vec1;
|
||||
|
||||
// Euroc
|
||||
vec1 << 460.76484651566468, 459.4051018049483, 365.8937161309615,
|
||||
249.33499869752445, 0.5903365915227143;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
// TUM VI 512
|
||||
vec1 << 191.14799816648748, 191.13150946585135, 254.95857715233118,
|
||||
256.8815466235898, 0.6291060871161842;
|
||||
res.emplace_back(vec1);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/// @brief Resolutions used for unit-tests
|
||||
static Eigen::aligned_vector<Eigen::Vector2i> getTestResolutions() {
|
||||
Eigen::aligned_vector<Eigen::Vector2i> res;
|
||||
|
||||
res.emplace_back(752, 480);
|
||||
res.emplace_back(512, 512);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
VecN param_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
Reference in New Issue
Block a user