ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

219
thirdparty/basalt-headers/include/basalt/calibration/calib_bias.hpp vendored Normal file
View File

@@ -0,0 +1,219 @@
/**
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 Definition of static IMU biases used for calibration
*/
#pragma once
#include <Eigen/Dense>
namespace basalt {
/// @brief Static calibration for accelerometer.
///
/// Calibrates axis scaling and misalignment and has 9 parameters \f$ [b_x,
/// b_y, b_z, s_1, s_2, s_3, s_4, s_5, s_6]^T \f$.
/// \f[
/// a_c = \begin{bmatrix} s_1 + 1 & 0 & 0 \\ s_2 & s_4 + 1 & 0 \\ s_3 & s_5 &
/// s_6 + 1 \\ \end{bmatrix} a_r - \begin{bmatrix} b_x \\ b_y \\ b_z
/// \end{bmatrix}
/// \f] where \f$ a_c \f$ is a calibrated measurement and \f$ a_r \f$ is a
/// raw measurement. When all elements are zero applying calibration results in
/// Identity mapping.
template <typename Scalar>
class CalibAccelBias {
public:
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using Mat33 = Eigen::Matrix<Scalar, 3, 3>;
/// @brief Default constructor with zero initialization.
inline CalibAccelBias() { accel_bias_full_.setZero(); }
/// @brief Set calibration to random values (used in unit-tests).
inline void setRandom() {
accel_bias_full_.setRandom();
accel_bias_full_.template head<3>() /= 10;
accel_bias_full_.template tail<6>() /= 100;
}
/// @brief Return const vector of parameters.
/// See detailed description in \ref CalibAccelBias.
inline const Eigen::Matrix<Scalar, 9, 1>& getParam() const {
return accel_bias_full_;
}
/// @brief Return vector of parameters. See detailed description in \ref
/// CalibAccelBias.
inline Eigen::Matrix<Scalar, 9, 1>& getParam() { return accel_bias_full_; }
/// @brief Increment the calibration vector
///
/// @param inc increment vector
inline void operator+=(const Eigen::Matrix<Scalar, 9, 1>& inc) {
accel_bias_full_ += inc;
}
/// @brief Return bias vector and scale matrix. See detailed description in
/// \ref CalibAccelBias.
inline void getBiasAndScale(Vec3& accel_bias, Mat33& accel_scale) const {
accel_bias = accel_bias_full_.template head<3>();
accel_scale.setZero();
accel_scale.col(0) = accel_bias_full_.template segment<3>(3);
accel_scale(1, 1) = accel_bias_full_(6);
accel_scale(2, 1) = accel_bias_full_(7);
accel_scale(2, 2) = accel_bias_full_(8);
}
/// @brief Calibrate the measurement. See detailed description in
/// \ref CalibAccelBias.
///
/// @param raw_measurement
/// @return calibrated measurement
inline Vec3 getCalibrated(const Vec3& raw_measurement) const {
Vec3 accel_bias;
Mat33 accel_scale;
getBiasAndScale(accel_bias, accel_scale);
return (raw_measurement + accel_scale * raw_measurement - accel_bias);
}
/// @brief Invert calibration (used in unit-tests).
///
/// @param calibrated_measurement
/// @return raw measurement
inline Vec3 invertCalibration(const Vec3& calibrated_measurement) const {
Vec3 accel_bias;
Mat33 accel_scale;
getBiasAndScale(accel_bias, accel_scale);
Mat33 accel_scale_inv =
(Eigen::Matrix3d::Identity() + accel_scale).inverse();
return accel_scale_inv * (calibrated_measurement + accel_bias);
}
private:
Eigen::Matrix<Scalar, 9, 1> accel_bias_full_;
};
/// @brief Static calibration for gyroscope.
///
/// Calibrates rotation, axis scaling and misalignment and has 12 parameters \f$
/// [b_x, b_y, b_z, s_1, s_2, s_3, s_4, s_5, s_6, s_7, s_8, s_9]^T \f$. \f[
/// \omega_c = \begin{bmatrix} s_1 + 1 & s_4 & s_7 \\ s_2 & s_5 + 1 & s_8 \\ s_3
/// & s_6 & s_9 +1 \\ \end{bmatrix} \omega_r - \begin{bmatrix} b_x \\ b_y
/// \\ b_z \end{bmatrix} \f] where \f$ \omega_c \f$ is a calibrated measurement
/// and \f$ \omega_r \f$ is a raw measurement. When all elements are zero
/// applying calibration results in Identity mapping.
template <typename Scalar>
class CalibGyroBias {
public:
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using Mat33 = Eigen::Matrix<Scalar, 3, 3>;
/// @brief Default constructor with zero initialization.
inline CalibGyroBias() { gyro_bias_full_.setZero(); }
/// @brief Set calibration to random values (used in unit-tests).
inline void setRandom() {
gyro_bias_full_.setRandom();
gyro_bias_full_.template head<3>() /= 10;
gyro_bias_full_.template tail<9>() /= 100;
}
/// @brief Return const vector of parameters.
/// See detailed description in \ref CalibGyroBias.
inline const Eigen::Matrix<Scalar, 12, 1>& getParam() const {
return gyro_bias_full_;
}
/// @brief Return vector of parameters.
/// See detailed description in \ref CalibGyroBias.
inline Eigen::Matrix<Scalar, 12, 1>& getParam() { return gyro_bias_full_; }
/// @brief Increment the calibration vector
///
/// @param inc increment vector
inline void operator+=(const Eigen::Matrix<Scalar, 12, 1>& inc) {
gyro_bias_full_ += inc;
}
/// @brief Return bias vector and scale matrix. See detailed description in
/// \ref CalibGyroBias.
inline void getBiasAndScale(Vec3& gyro_bias, Mat33& gyro_scale) const {
gyro_bias = gyro_bias_full_.template head<3>();
gyro_scale.col(0) = gyro_bias_full_.template segment<3>(3);
gyro_scale.col(1) = gyro_bias_full_.template segment<3>(6);
gyro_scale.col(2) = gyro_bias_full_.template segment<3>(9);
}
/// @brief Calibrate the measurement. See detailed description in
/// \ref CalibGyroBias.
///
/// @param raw_measurement
/// @return calibrated measurement
inline Vec3 getCalibrated(const Vec3& raw_measurement) const {
Vec3 gyro_bias;
Mat33 gyro_scale;
getBiasAndScale(gyro_bias, gyro_scale);
return (raw_measurement + gyro_scale * raw_measurement - gyro_bias);
}
/// @brief Invert calibration (used in unit-tests).
///
/// @param calibrated_measurement
/// @return raw measurement
inline Vec3 invertCalibration(const Vec3& calibrated_measurement) const {
Vec3 gyro_bias;
Mat33 gyro_scale;
getBiasAndScale(gyro_bias, gyro_scale);
Mat33 gyro_scale_inv = (Eigen::Matrix3d::Identity() + gyro_scale).inverse();
return gyro_scale_inv * (calibrated_measurement + gyro_bias);
}
private:
Eigen::Matrix<Scalar, 12, 1> gyro_bias_full_;
};
} // namespace basalt

194
thirdparty/basalt-headers/include/basalt/calibration/calibration.hpp vendored Normal file
View File

@@ -0,0 +1,194 @@
/**
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 Calibration datatypes for muticam-IMU and motion capture calibration
*/
#pragma once
#include <memory>
#include <basalt/spline/rd_spline.h>
#include <basalt/calibration/calib_bias.hpp>
#include <basalt/camera/generic_camera.hpp>
namespace basalt {
/// @brief Struct to store camera-IMU calibration
template <class Scalar>
struct Calibration {
using Ptr = std::shared_ptr<Calibration>;
using SE3 = Sophus::SE3<Scalar>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
/// @brief Default constructor.
Calibration() {
cam_time_offset_ns = 0;
imu_update_rate = 200;
// reasonable defaults
gyro_noise_std.setConstant(0.000282);
accel_noise_std.setConstant(0.016);
accel_bias_std.setConstant(0.001);
gyro_bias_std.setConstant(0.0001);
}
/// @brief Cast to other scalar type
template <class Scalar2>
Calibration<Scalar2> cast() const {
Calibration<Scalar2> new_cam;
for (const auto& v : T_i_c)
new_cam.T_i_c.emplace_back(v.template cast<Scalar2>());
for (const auto& v : intrinsics)
new_cam.intrinsics.emplace_back(v.template cast<Scalar2>());
for (const auto& v : vignette)
new_cam.vignette.emplace_back(v.template cast<Scalar2>());
new_cam.resolution = resolution;
new_cam.cam_time_offset_ns = cam_time_offset_ns;
new_cam.calib_accel_bias.getParam() =
calib_accel_bias.getParam().template cast<Scalar2>();
new_cam.calib_gyro_bias.getParam() =
calib_gyro_bias.getParam().template cast<Scalar2>();
new_cam.imu_update_rate = imu_update_rate;
new_cam.gyro_noise_std = gyro_noise_std.template cast<Scalar2>();
new_cam.accel_noise_std = accel_noise_std.template cast<Scalar2>();
new_cam.gyro_bias_std = gyro_bias_std.template cast<Scalar2>();
new_cam.accel_bias_std = accel_bias_std.template cast<Scalar2>();
return new_cam;
}
/// @brief Vector of transformations from camera to IMU
///
/// Point in camera coordinate frame \f$ p_c \f$ can be transformed to the
/// point in IMU coordinate frame as \f$ p_i = T_{ic} p_c, T_{ic} \in
/// SE(3)\f$
Eigen::aligned_vector<SE3> T_i_c;
/// @brief Vector of camera intrinsics. Can store different camera models. See
/// \ref GenericCamera.
Eigen::aligned_vector<GenericCamera<Scalar>> intrinsics;
/// @brief Camera resolutions.
Eigen::aligned_vector<Eigen::Vector2i> resolution;
/// @brief Vector of splines representing radially symmetric vignetting for
/// each of the camera.
///
/// Splines use time in nanoseconds for evaluation, but in this case we use
/// distance from the optical center in pixels multiplied by 1e9 as a "time"
/// parameter.
std::vector<basalt::RdSpline<1, 4, Scalar>> vignette;
/// @brief Time offset between cameras and IMU in nanoseconds.
///
/// With raw image timestamp \f$ t_r \f$ and this offset \f$ o \f$ we cam get
/// a timestamp aligned with IMU clock as \f$ t_c = t_r + o \f$.
int64_t cam_time_offset_ns;
/// @brief Static accelerometer bias from calibration.
CalibAccelBias<Scalar> calib_accel_bias;
/// @brief Static gyroscope bias from calibration.
CalibGyroBias<Scalar> calib_gyro_bias;
/// @brief IMU update rate.
Scalar imu_update_rate;
/// @brief Continuous time gyroscope noise standard deviation.
Vec3 gyro_noise_std;
/// @brief Continuous time accelerometer noise standard deviation.
Vec3 accel_noise_std;
/// @brief Continuous time bias random walk standard deviation for gyroscope.
Vec3 gyro_bias_std;
/// @brief Continuous time bias random walk standard deviation for
/// accelerometer.
Vec3 accel_bias_std;
/// @brief Dicrete time gyroscope noise standard deviation.
///
/// \f$ \sigma_d = \sigma_c \sqrt{r} \f$, where \f$ r \f$ is IMU update
/// rate.
inline Vec3 dicrete_time_gyro_noise_std() const {
return gyro_noise_std * std::sqrt(imu_update_rate);
}
/// @brief Dicrete time accelerometer noise standard deviation.
///
/// \f$ \sigma_d = \sigma_c \sqrt{r} \f$, where \f$ r \f$ is IMU update
/// rate.
inline Vec3 dicrete_time_accel_noise_std() const {
return accel_noise_std * std::sqrt(imu_update_rate);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/// @brief Struct to store motion capture to IMU calibration
template <class Scalar>
struct MocapCalibration {
using Ptr = std::shared_ptr<MocapCalibration>;
using SE3 = Sophus::SE3<Scalar>;
/// @brief Default constructor.
MocapCalibration() {
mocap_time_offset_ns = 0;
mocap_to_imu_offset_ns = 0;
}
/// @brief Transformation from motion capture origin to the world (calibration
/// pattern).
SE3 T_moc_w;
/// @brief Transformation from the coordinate frame of the markers attached to
/// the object to the IMU.
SE3 T_i_mark;
/// @brief Initial time alignment between IMU and MoCap clocks based on
/// message arrival time.
int64_t mocap_time_offset_ns;
/// @brief Time offset between IMU and motion capture clock.
int64_t mocap_to_imu_offset_ns;
};
} // namespace basalt

303
thirdparty/basalt-headers/include/basalt/camera/bal_camera.hpp vendored Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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

615
thirdparty/basalt-headers/include/basalt/image/image.h vendored Normal file
View File

@@ -0,0 +1,615 @@
/**
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 Image datatype with views, interpolation, gradients
*/
// This file is adapted from Pangolin. Original license:
/* This file is part of the Pangolin Project.
* http://github.com/stevenlovegrove/Pangolin
*
* Copyright (c) 2011 Steven Lovegrove
*
* Permission is hereby granted, free of charge, to any person
* obtaining a copy of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use,
* copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following
* conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*/
#pragma once
#include <memory>
#include <Eigen/Dense>
#include <basalt/utils/assert.h>
// Renamed Pangoling defines to avoid clash
#define BASALT_HOST_DEVICE
#define BASALT_EXTENSION_IMAGE
#ifdef BASALT_ENABLE_BOUNDS_CHECKS
#define BASALT_BOUNDS_ASSERT(...) BASALT_ASSERT(##__VA_ARGS__)
#else
#define BASALT_BOUNDS_ASSERT(...) ((void)0)
#endif
namespace basalt {
/// @brief Helper class for copying objects.
template <typename T>
struct CopyObject {
CopyObject(const T& obj) : obj(obj) {}
const T& obj;
};
inline void PitchedCopy(char* dst, unsigned int dst_pitch_bytes,
const char* src, unsigned int src_pitch_bytes,
unsigned int width_bytes, unsigned int height) {
if (dst_pitch_bytes == width_bytes && src_pitch_bytes == width_bytes) {
std::memcpy(dst, src, height * width_bytes);
} else {
for (unsigned int row = 0; row < height; ++row) {
std::memcpy(dst, src, width_bytes);
dst += dst_pitch_bytes;
src += src_pitch_bytes;
}
}
}
/// @brief Image class that supports sub-images, interpolation, element access.
template <typename T>
struct Image {
using PixelType = T;
inline Image() : pitch(0), ptr(0), w(0), h(0) {}
inline Image(T* ptr, size_t w, size_t h, size_t pitch)
: pitch(pitch), ptr(ptr), w(w), h(h) {}
BASALT_HOST_DEVICE inline size_t SizeBytes() const { return pitch * h; }
BASALT_HOST_DEVICE inline size_t Area() const { return w * h; }
BASALT_HOST_DEVICE inline bool IsValid() const { return ptr != 0; }
BASALT_HOST_DEVICE inline bool IsContiguous() const {
return w * sizeof(T) == pitch;
}
//////////////////////////////////////////////////////
// Iterators
//////////////////////////////////////////////////////
BASALT_HOST_DEVICE inline T* begin() { return ptr; }
BASALT_HOST_DEVICE inline T* end() { return RowPtr(h - 1) + w; }
BASALT_HOST_DEVICE inline const T* begin() const { return ptr; }
BASALT_HOST_DEVICE inline const T* end() const { return RowPtr(h - 1) + w; }
BASALT_HOST_DEVICE inline size_t size() const { return w * h; }
//////////////////////////////////////////////////////
// Image transforms
//////////////////////////////////////////////////////
template <typename UnaryOperation>
BASALT_HOST_DEVICE inline void Transform(UnaryOperation unary_op) {
BASALT_ASSERT(IsValid());
for (size_t y = 0; y < h; ++y) {
T* el = RowPtr(y);
const T* el_end = el + w;
for (; el != el_end; ++el) {
*el = unary_op(*el);
}
}
}
BASALT_HOST_DEVICE inline void Fill(const T& val) {
Transform([&](const T&) { return val; });
}
BASALT_HOST_DEVICE inline void Replace(const T& oldval, const T& newval) {
Transform([&](const T& val) { return (val == oldval) ? newval : val; });
}
inline void Memset(unsigned char v = 0) {
BASALT_ASSERT(IsValid());
if (IsContiguous()) {
std::memset((char*)ptr, v, pitch * h);
} else {
for (size_t y = 0; y < h; ++y) {
std::memset((char*)RowPtr(y), v, pitch);
}
}
}
inline void CopyFrom(const Image<T>& img) {
if (IsValid() && img.IsValid()) {
BASALT_ASSERT(w >= img.w && h >= img.h);
PitchedCopy((char*)ptr, pitch, (char*)img.ptr, img.pitch,
std::min(img.w, w) * sizeof(T), std::min(img.h, h));
} else if (img.IsValid() != IsValid()) {
BASALT_ASSERT(false && "Cannot copy from / to an unasigned image.");
}
}
//////////////////////////////////////////////////////
// Reductions
//////////////////////////////////////////////////////
template <typename BinaryOperation>
BASALT_HOST_DEVICE inline T Accumulate(const T init,
BinaryOperation binary_op) {
BASALT_ASSERT(IsValid());
T val = init;
for (size_t y = 0; y < h; ++y) {
T* el = RowPtr(y);
const T* el_end = el + w;
for (; el != el_end; ++el) {
val = binary_op(val, *el);
}
}
return val;
}
std::pair<T, T> MinMax() const {
BASALT_ASSERT(IsValid());
std::pair<T, T> minmax(std::numeric_limits<T>::max(),
std::numeric_limits<T>::lowest());
for (size_t r = 0; r < h; ++r) {
const T* ptr = RowPtr(r);
const T* end = ptr + w;
while (ptr != end) {
minmax.first = std::min(*ptr, minmax.first);
minmax.second = std::max(*ptr, minmax.second);
++ptr;
}
}
return minmax;
}
template <typename Tout = T>
Tout Sum() const {
return Accumulate((T)0,
[](const T& lhs, const T& rhs) { return lhs + rhs; });
}
template <typename Tout = T>
Tout Mean() const {
return Sum<Tout>() / Area();
}
//////////////////////////////////////////////////////
// Direct Pixel Access
//////////////////////////////////////////////////////
BASALT_HOST_DEVICE inline T* RowPtr(size_t y) {
return (T*)((unsigned char*)(ptr) + y * pitch);
}
BASALT_HOST_DEVICE inline const T* RowPtr(size_t y) const {
return (T*)((unsigned char*)(ptr) + y * pitch);
}
BASALT_HOST_DEVICE inline T& operator()(size_t x, size_t y) {
BASALT_BOUNDS_ASSERT(InBounds(x, y));
return RowPtr(y)[x];
}
BASALT_HOST_DEVICE inline const T& operator()(size_t x, size_t y) const {
BASALT_BOUNDS_ASSERT(InBounds(x, y));
return RowPtr(y)[x];
}
template <typename TVec>
BASALT_HOST_DEVICE inline T& operator()(const TVec& p) {
BASALT_BOUNDS_ASSERT(InBounds(p[0], p[1]));
return RowPtr(p[1])[p[0]];
}
template <typename TVec>
BASALT_HOST_DEVICE inline const T& operator()(const TVec& p) const {
BASALT_BOUNDS_ASSERT(InBounds(p[0], p[1]));
return RowPtr(p[1])[p[0]];
}
BASALT_HOST_DEVICE inline T& operator[](size_t ix) {
BASALT_BOUNDS_ASSERT(InImage(ptr + ix));
return ptr[ix];
}
BASALT_HOST_DEVICE inline const T& operator[](size_t ix) const {
BASALT_BOUNDS_ASSERT(InImage(ptr + ix));
return ptr[ix];
}
//////////////////////////////////////////////////////
// Interpolated Pixel Access
//////////////////////////////////////////////////////
template <typename S>
inline S interp(const Eigen::Matrix<S, 2, 1>& p) const {
return interp<S>(p[0], p[1]);
}
template <typename S>
inline Eigen::Matrix<S, 3, 1> interpGrad(
const Eigen::Matrix<S, 2, 1>& p) const {
return interpGrad<S>(p[0], p[1]);
}
template <typename S>
inline S interp(S x, S y) const {
static_assert(std::is_floating_point_v<S>,
"interpolation / gradient only makes sense "
"for floating point result type");
int ix = x;
int iy = y;
S dx = x - ix;
S dy = y - iy;
S ddx = S(1.0) - dx;
S ddy = S(1.0) - dy;
return ddx * ddy * (*this)(ix, iy) + ddx * dy * (*this)(ix, iy + 1) +
dx * ddy * (*this)(ix + 1, iy) + dx * dy * (*this)(ix + 1, iy + 1);
}
template <typename S>
inline Eigen::Matrix<S, 3, 1> interpGrad(S x, S y) const {
static_assert(std::is_floating_point_v<S>,
"interpolation / gradient only makes sense "
"for floating point result type");
int ix = x;
int iy = y;
S dx = x - ix;
S dy = y - iy;
S ddx = S(1.0) - dx;
S ddy = S(1.0) - dy;
Eigen::Matrix<S, 3, 1> res;
const T& px0y0 = (*this)(ix, iy);
const T& px1y0 = (*this)(ix + 1, iy);
const T& px0y1 = (*this)(ix, iy + 1);
const T& px1y1 = (*this)(ix + 1, iy + 1);
res[0] = ddx * ddy * px0y0 + ddx * dy * px0y1 + dx * ddy * px1y0 +
dx * dy * px1y1;
const T& pxm1y0 = (*this)(ix - 1, iy);
const T& pxm1y1 = (*this)(ix - 1, iy + 1);
S res_mx = ddx * ddy * pxm1y0 + ddx * dy * pxm1y1 + dx * ddy * px0y0 +
dx * dy * px0y1;
const T& px2y0 = (*this)(ix + 2, iy);
const T& px2y1 = (*this)(ix + 2, iy + 1);
S res_px = ddx * ddy * px1y0 + ddx * dy * px1y1 + dx * ddy * px2y0 +
dx * dy * px2y1;
res[1] = S(0.5) * (res_px - res_mx);
const T& px0ym1 = (*this)(ix, iy - 1);
const T& px1ym1 = (*this)(ix + 1, iy - 1);
S res_my = ddx * ddy * px0ym1 + ddx * dy * px0y0 + dx * ddy * px1ym1 +
dx * dy * px1y0;
const T& px0y2 = (*this)(ix, iy + 2);
const T& px1y2 = (*this)(ix + 1, iy + 2);
S res_py = ddx * ddy * px0y1 + ddx * dy * px0y2 + dx * ddy * px1y1 +
dx * dy * px1y2;
res[2] = S(0.5) * (res_py - res_my);
return res;
}
//////////////////////////////////////////////////////
// Bounds Checking
//////////////////////////////////////////////////////
BASALT_HOST_DEVICE
bool InImage(const T* ptest) const {
return ptr <= ptest && ptest < RowPtr(h);
}
BASALT_HOST_DEVICE inline bool InBounds(int x, int y) const {
return 0 <= x && x < (int)w && 0 <= y && y < (int)h;
}
BASALT_HOST_DEVICE inline bool InBounds(float x, float y,
float border) const {
return border <= x && x < (w - border - 1) && border <= y &&
y < (h - border - 1);
}
template <typename Derived>
BASALT_HOST_DEVICE inline bool InBounds(
const Eigen::MatrixBase<Derived>& p,
const typename Derived::Scalar border) const {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 2);
using Scalar = typename Derived::Scalar;
Scalar offset(0);
if constexpr (std::is_floating_point_v<Scalar>) {
offset = Scalar(1);
}
return border <= p[0] && p[0] < ((int)w - border - offset) &&
border <= p[1] && p[1] < ((int)h - border - offset);
}
//////////////////////////////////////////////////////
// Obtain slices / subimages
//////////////////////////////////////////////////////
BASALT_HOST_DEVICE inline const Image<const T> SubImage(size_t x, size_t y,
size_t width,
size_t height) const {
BASALT_ASSERT((x + width) <= w && (y + height) <= h);
return Image<const T>(RowPtr(y) + x, width, height, pitch);
}
BASALT_HOST_DEVICE inline Image<T> SubImage(size_t x, size_t y, size_t width,
size_t height) {
BASALT_ASSERT((x + width) <= w && (y + height) <= h);
return Image<T>(RowPtr(y) + x, width, height, pitch);
}
BASALT_HOST_DEVICE inline Image<T> Row(int y) const {
return SubImage(0, y, w, 1);
}
BASALT_HOST_DEVICE inline Image<T> Col(int x) const {
return SubImage(x, 0, 1, h);
}
//////////////////////////////////////////////////////
// Data mangling
//////////////////////////////////////////////////////
template <typename TRecast>
BASALT_HOST_DEVICE inline Image<TRecast> Reinterpret() const {
BASALT_ASSERT_STREAM(sizeof(TRecast) == sizeof(T),
"sizeof(TRecast) must match sizeof(T): "
<< sizeof(TRecast) << " != " << sizeof(T));
return UnsafeReinterpret<TRecast>();
}
template <typename TRecast>
BASALT_HOST_DEVICE inline Image<TRecast> UnsafeReinterpret() const {
return Image<TRecast>((TRecast*)ptr, w, h, pitch);
}
//////////////////////////////////////////////////////
// Deprecated methods
//////////////////////////////////////////////////////
// PANGOLIN_DEPRECATED inline
Image(size_t w, size_t h, size_t pitch, T* ptr)
: pitch(pitch), ptr(ptr), w(w), h(h) {}
// Use RAII/move aware pangolin::ManagedImage instead
// PANGOLIN_DEPRECATED inline
void Dealloc() {
if (ptr) {
::operator delete(ptr);
ptr = nullptr;
}
}
// Use RAII/move aware pangolin::ManagedImage instead
// PANGOLIN_DEPRECATED inline
void Alloc(size_t w, size_t h, size_t pitch) {
Dealloc();
this->w = w;
this->h = h;
this->pitch = pitch;
this->ptr = (T*)::operator new(h* pitch);
}
//////////////////////////////////////////////////////
// Data members
//////////////////////////////////////////////////////
size_t pitch;
T* ptr;
size_t w;
size_t h;
BASALT_EXTENSION_IMAGE
};
template <class T>
using DefaultImageAllocator = std::allocator<T>;
/// @brief Image that manages it's own memory, storing a strong pointer to it's
/// memory
template <typename T, class Allocator = DefaultImageAllocator<T>>
class ManagedImage : public Image<T> {
public:
using PixelType = T;
using Ptr = std::shared_ptr<ManagedImage<T, Allocator>>;
// Destructor
inline ~ManagedImage() { Deallocate(); }
// Null image
inline ManagedImage() {}
// Row image
inline ManagedImage(size_t w)
: Image<T>(Allocator().allocate(w), w, 1, w * sizeof(T)) {}
inline ManagedImage(size_t w, size_t h)
: Image<T>(Allocator().allocate(w * h), w, h, w * sizeof(T)) {}
inline ManagedImage(size_t w, size_t h, size_t pitch_bytes)
: Image<T>(Allocator().allocate((h * pitch_bytes) / sizeof(T) + 1), w, h,
pitch_bytes) {}
// Not copy constructable
inline ManagedImage(const ManagedImage<T>& other) = delete;
// Move constructor
inline ManagedImage(ManagedImage<T, Allocator>&& img) {
*this = std::move(img);
}
// Move asignment
inline void operator=(ManagedImage<T, Allocator>&& img) {
Deallocate();
Image<T>::pitch = img.pitch;
Image<T>::ptr = img.ptr;
Image<T>::w = img.w;
Image<T>::h = img.h;
img.ptr = nullptr;
}
// Explicit copy constructor
template <typename TOther>
ManagedImage(const CopyObject<TOther>& other) {
CopyFrom(other.obj);
}
// Explicit copy assignment
template <typename TOther>
void operator=(const CopyObject<TOther>& other) {
CopyFrom(other.obj);
}
inline void Swap(ManagedImage<T>& img) {
std::swap(img.pitch, Image<T>::pitch);
std::swap(img.ptr, Image<T>::ptr);
std::swap(img.w, Image<T>::w);
std::swap(img.h, Image<T>::h);
}
inline void CopyFrom(const Image<T>& img) {
if (!Image<T>::IsValid() || Image<T>::w != img.w || Image<T>::h != img.h) {
Reinitialise(img.w, img.h);
}
Image<T>::CopyFrom(img);
}
inline void Reinitialise(size_t w, size_t h) {
if (!Image<T>::ptr || Image<T>::w != w || Image<T>::h != h) {
*this = ManagedImage<T, Allocator>(w, h);
}
}
inline void Reinitialise(size_t w, size_t h, size_t pitch) {
if (!Image<T>::ptr || Image<T>::w != w || Image<T>::h != h ||
Image<T>::pitch != pitch) {
*this = ManagedImage<T, Allocator>(w, h, pitch);
}
}
inline void Deallocate() {
if (Image<T>::ptr) {
Allocator().deallocate(Image<T>::ptr,
(Image<T>::h * Image<T>::pitch) / sizeof(T));
Image<T>::ptr = nullptr;
}
}
// Move asignment
template <typename TOther, typename AllocOther>
inline void OwnAndReinterpret(ManagedImage<TOther, AllocOther>&& img) {
Deallocate();
Image<T>::pitch = img.pitch;
Image<T>::ptr = (T*)img.ptr;
Image<T>::w = img.w;
Image<T>::h = img.h;
img.ptr = nullptr;
}
template <typename T1>
inline void ConvertFrom(const ManagedImage<T1>& img) {
Reinitialise(img.w, img.h);
for (size_t j = 0; j < img.h; j++) {
T* this_row = this->RowPtr(j);
const T1* other_row = img.RowPtr(j);
for (size_t i = 0; i < img.w; i++) {
this_row[i] = T(other_row[i]);
}
}
}
inline void operator-=(const ManagedImage<T>& img) {
for (size_t j = 0; j < img.h; j++) {
T* this_row = this->RowPtr(j);
const T* other_row = img.RowPtr(j);
for (size_t i = 0; i < img.w; i++) {
this_row[i] -= other_row[i];
}
}
}
};
} // namespace basalt

200
thirdparty/basalt-headers/include/basalt/image/image_pyr.h vendored Normal file
View File

@@ -0,0 +1,200 @@
/**
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 Image pyramid implementation stored as mipmap
*/
#pragma once
#include <basalt/image/image.h>
namespace basalt {
/// @brief Image pyramid that stores levels as mipmap
///
/// \image html mipmap.jpeg
/// Computes image pyramid (see \ref subsample) and stores it as a mipmap
/// (https://en.wikipedia.org/wiki/Mipmap).
template <typename T, class Allocator = DefaultImageAllocator<T>>
class ManagedImagePyr {
public:
using PixelType = T;
using Ptr = std::shared_ptr<ManagedImagePyr<T, Allocator>>;
/// @brief Default constructor.
inline ManagedImagePyr() {}
/// @brief Construct image pyramid from other image.
///
/// @param other image to use for the pyramid level 0
/// @param num_level number of levels for the pyramid
inline ManagedImagePyr(const ManagedImage<T>& other, size_t num_levels) {
setFromImage(other, num_levels);
}
/// @brief Set image pyramid from other image.
///
/// @param other image to use for the pyramid level 0
/// @param num_level number of levels for the pyramid
inline void setFromImage(const ManagedImage<T>& other, size_t num_levels) {
orig_w = other.w;
image.Reinitialise(other.w + other.w / 2, other.h);
image.Fill(0);
lvl_internal(0).CopyFrom(other);
for (size_t i = 0; i < num_levels; i++) {
const Image<const T> l = lvl(i);
Image<T> lp1 = lvl_internal(i + 1);
subsample(l, lp1);
}
}
/// @brief Extrapolate image after border with reflection.
static inline int border101(int x, int h) {
return h - 1 - std::abs(h - 1 - x);
}
/// @brief Subsample the image twice in each direction.
///
/// Subsampling is done by convolution with Gaussian kernel
/// \f[
/// \frac{1}{256}
/// \begin{bmatrix}
/// 1 & 4 & 6 & 4 & 1
/// \\4 & 16 & 24 & 16 & 4
/// \\6 & 24 & 36 & 24 & 6
/// \\4 & 16 & 24 & 16 & 4
/// \\1 & 4 & 6 & 4 & 1
/// \\ \end{bmatrix}
/// \f]
/// and removing every even-numbered row and column.
static void subsample(const Image<const T>& img, Image<T>& img_sub) {
static_assert(std::is_same<T, uint16_t>::value ||
std::is_same<T, uint8_t>::value);
constexpr int kernel[5] = {1, 4, 6, 4, 1};
// accumulator
ManagedImage<int> tmp(img_sub.h, img.w);
// Vertical convolution
{
for (int r = 0; r < int(img_sub.h); r++) {
const T* row_m2 = img.RowPtr(std::abs(2 * r - 2));
const T* row_m1 = img.RowPtr(std::abs(2 * r - 1));
const T* row = img.RowPtr(2 * r);
const T* row_p1 = img.RowPtr(border101(2 * r + 1, img.h));
const T* row_p2 = img.RowPtr(border101(2 * r + 2, img.h));
for (int c = 0; c < int(img.w); c++) {
tmp(r, c) = kernel[0] * int(row_m2[c]) + kernel[1] * int(row_m1[c]) +
kernel[2] * int(row[c]) + kernel[3] * int(row_p1[c]) +
kernel[4] * int(row_p2[c]);
}
}
}
// Horizontal convolution
{
for (int c = 0; c < int(img_sub.w); c++) {
const int* row_m2 = tmp.RowPtr(std::abs(2 * c - 2));
const int* row_m1 = tmp.RowPtr(std::abs(2 * c - 1));
const int* row = tmp.RowPtr(2 * c);
const int* row_p1 = tmp.RowPtr(border101(2 * c + 1, tmp.h));
const int* row_p2 = tmp.RowPtr(border101(2 * c + 2, tmp.h));
for (int r = 0; r < int(tmp.w); r++) {
int val_int = kernel[0] * row_m2[r] + kernel[1] * row_m1[r] +
kernel[2] * row[r] + kernel[3] * row_p1[r] +
kernel[4] * row_p2[r];
T val = ((val_int + (1 << 7)) >> 8);
img_sub(c, r) = val;
}
}
}
}
/// @brief Return const image of the certain level
///
/// @param lvl level to return
/// @return const image of with the pyramid level
inline const Image<const T> lvl(size_t lvl) const {
size_t x = (lvl == 0) ? 0 : orig_w;
size_t y = (lvl <= 1) ? 0 : (image.h - (image.h >> (lvl - 1)));
size_t width = (orig_w >> lvl);
size_t height = (image.h >> lvl);
return image.SubImage(x, y, width, height);
}
/// @brief Return const image of underlying mipmap
///
/// @return const image of of the underlying mipmap representation which can
/// be for example used for visualization
inline const Image<const T> mipmap() const {
return image.SubImage(0, 0, image.w, image.h);
}
/// @brief Return coordinate offset of the image in the mipmap image.
///
/// @param lvl level to return
/// @return offset coordinates (2x1 vector)
template <typename S>
inline Eigen::Matrix<S, 2, 1> lvl_offset(size_t lvl) {
size_t x = (lvl == 0) ? 0 : orig_w;
size_t y = (lvl <= 1) ? 0 : (image.h - (image.h >> (lvl - 1)));
return Eigen::Matrix<S, 2, 1>(x, y);
}
protected:
/// @brief Return image of the certain level
///
/// @param lvl level to return
/// @return image of with the pyramid level
inline Image<T> lvl_internal(size_t lvl) {
size_t x = (lvl == 0) ? 0 : orig_w;
size_t y = (lvl <= 1) ? 0 : (image.h - (image.h >> (lvl - 1)));
size_t width = (orig_w >> lvl);
size_t height = (image.h >> lvl);
return image.SubImage(x, y, width, height);
}
size_t orig_w; ///< Width of the original image (level 0)
ManagedImage<T> image; ///< Pyramid image stored as a mipmap
};
} // namespace basalt

283
thirdparty/basalt-headers/include/basalt/imu/imu_types.h vendored Normal file
View File

@@ -0,0 +1,283 @@
/**
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 Type definitions for IMU preintegration
*/
#pragma once
#include <basalt/utils/sophus_utils.hpp>
#include <memory>
namespace basalt {
constexpr size_t POSE_SIZE = 6; ///< Dimentionality of the pose state
constexpr size_t POSE_VEL_SIZE =
9; ///< Dimentionality of the pose-velocity state
constexpr size_t POSE_VEL_BIAS_SIZE =
15; ///< Dimentionality of the pose-velocity-bias state
/// @brief State that consists of SE(3) pose at a certain time.
template <class Scalar_>
struct PoseState {
using Scalar = Scalar_;
using VecN = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
using Vec6 = Eigen::Matrix<Scalar, 6, 1>;
using SO3 = Sophus::SO3<Scalar>;
using SE3 = Sophus::SE3<Scalar>;
/// @brief Default constructor with Identity pose and zero timestamp.
PoseState() { t_ns = 0; }
/// @brief Constructor with timestamp and pose.
///
/// @param t_ns timestamp of the state in nanoseconds
/// @param T_w_i transformation from the body frame to the world frame
PoseState(int64_t t_ns, const SE3& T_w_i) : t_ns(t_ns), T_w_i(T_w_i) {}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
PoseState<Scalar2> cast() const {
PoseState<Scalar2> a;
a.t_ns = t_ns;
a.T_w_i = T_w_i.template cast<Scalar2>();
return a;
}
/// @brief Apply increment to the pose
///
/// For details see \ref incPose
/// @param[in] inc 6x1 increment vector
void applyInc(const VecN& inc) { incPose(inc, T_w_i); }
/// @brief Apply increment to the pose
///
/// The incremernt vector consists of translational and rotational parts \f$
/// [\upsilon, \omega]^T \f$. Given the current pose \f$ R \in
/// SO(3), p \in \mathbb{R}^3\f$ the updated pose is: \f{align}{ R' &=
/// \exp(\omega) R
/// \\ p' &= p + \upsilon
/// \f}
/// The increment is consistent with \ref
/// Se3Spline::applyInc.
///
/// @param[in] inc 6x1 increment vector
/// @param[in,out] T the pose to update
inline static void incPose(const Vec6& inc, SE3& T) {
T.translation() += inc.template head<3>();
T.so3() = SO3::exp(inc.template tail<3>()) * T.so3();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
int64_t t_ns; ///< timestamp of the state in nanoseconds
SE3 T_w_i; ///< pose of the state
};
/// @brief State that consists of SE(3) pose and linear velocity at a certain
/// time.
template <class Scalar_>
struct PoseVelState : public PoseState<Scalar_> {
using Scalar = Scalar_;
using VecN = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 1>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using SE3 = Sophus::SE3<Scalar>;
/// @brief Default constructor with Identity pose and zero other values.
PoseVelState() { vel_w_i.setZero(); };
/// @brief Constructor with timestamp, pose and linear velocity.
///
/// @param t_ns timestamp of the state in nanoseconds
/// @param T_w_i transformation from the body frame to the world frame
/// @param vel_w_i linear velocity in world coordinate frame
PoseVelState(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i)
: PoseState<Scalar>(t_ns, T_w_i), vel_w_i(vel_w_i) {}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
PoseVelState<Scalar2> cast() const {
PoseVelState<Scalar2> a;
static_cast<PoseState<Scalar2>&>(a) =
PoseState<Scalar>::template cast<Scalar2>();
a.vel_w_i = vel_w_i.template cast<Scalar2>();
return a;
}
/// @brief Apply increment to the state.
///
/// For pose see \ref incPose. For velocity simple addition.
/// @param[in] inc 9x1 increment vector [trans, rot, vel]
void applyInc(const VecN& inc) {
PoseState<Scalar>::applyInc(inc.template head<6>());
vel_w_i += inc.template tail<3>();
}
/// @brief Compute difference to other state.
///
/// ```
/// PoseVelState::VecN inc;
/// PoseVelState p0, p1;
/// // Initialize p0 and inc
/// p1 = p0;
/// p1.applyInc(inc);
/// p0.diff(p1) == inc; // Should be true.
/// ```
/// @param other state to compute difference.
VecN diff(const PoseVelState<Scalar>& other) const {
VecN res;
res.template segment<3>(0) =
other.T_w_i.translation() - this->T_w_i.translation();
res.template segment<3>(3) =
(other.T_w_i.so3() * this->T_w_i.so3().inverse()).log();
res.template tail<3>() = other.vel_w_i - vel_w_i;
return res;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vec3 vel_w_i; ///< Linear velocity of the state
};
/// @brief State that consists of SE(3) pose, linear velocity, gyroscope and
/// accelerometer biases at a certain time.
template <class Scalar_>
struct PoseVelBiasState : public PoseVelState<Scalar_> {
using Scalar = Scalar_;
using Ptr = std::shared_ptr<PoseVelBiasState<Scalar>>;
using VecN = Eigen::Matrix<Scalar, POSE_VEL_BIAS_SIZE, 1>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using SO3 = Sophus::SO3<Scalar>;
using SE3 = Sophus::SE3<Scalar>;
/// @brief Default constructor with Identity pose and zero other values.
PoseVelBiasState() {
bias_gyro.setZero();
bias_accel.setZero();
};
/// @brief Constructor with timestamp, pose, linear velocity, gyroscope and
/// accelerometer biases.
///
/// @param t_ns timestamp of the state in nanoseconds
/// @param T_w_i transformation from the body frame to the world frame
/// @param vel_w_i linear velocity in world coordinate frame
/// @param bias_gyro gyroscope bias
/// @param bias_accel accelerometer bias
PoseVelBiasState(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i,
const Vec3& bias_gyro, const Vec3& bias_accel)
: PoseVelState<Scalar>(t_ns, T_w_i, vel_w_i),
bias_gyro(bias_gyro),
bias_accel(bias_accel) {}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
PoseVelBiasState<Scalar2> cast() const {
PoseVelBiasState<Scalar2> a;
static_cast<PoseVelState<Scalar2>&>(a) =
PoseVelState<Scalar>::template cast<Scalar2>();
a.bias_gyro = bias_gyro.template cast<Scalar2>();
a.bias_accel = bias_accel.template cast<Scalar2>();
return a;
}
/// @brief Apply increment to the state.
///
/// For pose see \ref incPose. For velocity and biases simple addition.
/// @param[in] inc 15x1 increment vector [trans, rot, vel, bias_gyro,
/// bias_accel]
void applyInc(const VecN& inc) {
PoseVelState<Scalar>::applyInc(inc.template head<9>());
bias_gyro += inc.template segment<3>(9);
bias_accel += inc.template segment<3>(12);
}
/// @brief Compute difference to other state.
///
/// ```
/// PoseVelBiasState::VecN inc;
/// PoseVelBiasState p0, p1;
/// // Initialize p0 and inc
/// p1 = p0;
/// p1.applyInc(inc);
/// p0.diff(p1) == inc; // Should be true.
/// ```
/// @param other state to compute difference.
VecN diff(const PoseVelBiasState<Scalar>& other) const {
VecN res;
res.segment<9>(0) = PoseVelState<Scalar>::diff(other);
res.segment<3>(9) = other.bias_gyro - bias_gyro;
res.segment<3>(12) = other.bias_accel - bias_accel;
return res;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vec3 bias_gyro; ///< Gyroscope bias
Vec3 bias_accel; ///< Accelerometer bias
};
/// @brief Timestamped gyroscope and accelerometer measurements.
template <class Scalar_>
struct ImuData {
using Scalar = Scalar_;
using Ptr = std::shared_ptr<ImuData>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
int64_t t_ns; ///< timestamp in nanoseconds
Vec3 accel; ///< Accelerometer measurement
Vec3 gyro; ///< Gyroscope measurement
/// @brief Default constructor with zero measurements.
ImuData() {
t_ns = 0;
accel.setZero();
gyro.setZero();
}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
ImuData<Scalar2> cast() const {
ImuData<Scalar2> a;
a.t_ns = t_ns;
a.accel = accel.template cast<Scalar2>();
a.gyro = gyro.template cast<Scalar2>();
return a;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace basalt

371
thirdparty/basalt-headers/include/basalt/imu/preintegration.h vendored Normal file
View File

@@ -0,0 +1,371 @@
/**
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 IMU preintegration
*/
#pragma once
#include <basalt/imu/imu_types.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/sophus_utils.hpp>
namespace basalt {
/// @brief Integrated pseudo-measurement that combines several consecutive IMU
/// measurements.
template <class Scalar_>
class IntegratedImuMeasurement {
public:
using Scalar = Scalar_;
using Ptr = std::shared_ptr<IntegratedImuMeasurement<Scalar>>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using VecN = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 1>;
using Mat3 = Eigen::Matrix<Scalar, 3, 3>;
using MatNN = Eigen::Matrix<Scalar, POSE_VEL_SIZE, POSE_VEL_SIZE>;
using MatN3 = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 3>;
using MatN6 = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 6>;
using SO3 = Sophus::SO3<Scalar>;
/// @brief Propagate current state given ImuData and optionally compute
/// Jacobians.
///
/// @param[in] curr_state current state
/// @param[in] data IMU data
/// @param[out] next_state predicted state
/// @param[out] d_next_d_curr Jacobian of the predicted state with respect
/// to current state
/// @param[out] d_next_d_accel Jacobian of the predicted state with respect
/// accelerometer measurement
/// @param[out] d_next_d_gyro Jacobian of the predicted state with respect
/// gyroscope measurement
inline static void propagateState(const PoseVelState<Scalar>& curr_state,
const ImuData<Scalar>& data,
PoseVelState<Scalar>& next_state,
MatNN* d_next_d_curr = nullptr,
MatN3* d_next_d_accel = nullptr,
MatN3* d_next_d_gyro = nullptr) {
BASALT_ASSERT_STREAM(
data.t_ns > curr_state.t_ns,
"data.t_ns " << data.t_ns << " curr_state.t_ns " << curr_state.t_ns);
int64_t dt_ns = data.t_ns - curr_state.t_ns;
Scalar dt = dt_ns * Scalar(1e-9);
SO3 R_w_i_new_2 =
curr_state.T_w_i.so3() * SO3::exp(Scalar(0.5) * dt * data.gyro);
Mat3 RR_w_i_new_2 = R_w_i_new_2.matrix();
Vec3 accel_world = RR_w_i_new_2 * data.accel;
next_state.t_ns = data.t_ns;
next_state.T_w_i.so3() = curr_state.T_w_i.so3() * SO3::exp(dt * data.gyro);
next_state.vel_w_i = curr_state.vel_w_i + accel_world * dt;
next_state.T_w_i.translation() = curr_state.T_w_i.translation() +
curr_state.vel_w_i * dt +
0.5 * accel_world * dt * dt;
if (d_next_d_curr) {
d_next_d_curr->setIdentity();
d_next_d_curr->template block<3, 3>(0, 6).diagonal().setConstant(dt);
d_next_d_curr->template block<3, 3>(6, 3) = SO3::hat(-accel_world * dt);
d_next_d_curr->template block<3, 3>(0, 3) =
d_next_d_curr->template block<3, 3>(6, 3) * dt * Scalar(0.5);
}
if (d_next_d_accel) {
d_next_d_accel->setZero();
d_next_d_accel->template block<3, 3>(0, 0) =
Scalar(0.5) * RR_w_i_new_2 * dt * dt;
d_next_d_accel->template block<3, 3>(6, 0) = RR_w_i_new_2 * dt;
}
if (d_next_d_gyro) {
d_next_d_gyro->setZero();
Mat3 Jr;
Sophus::rightJacobianSO3(dt * data.gyro, Jr);
Mat3 Jr2;
Sophus::rightJacobianSO3(Scalar(0.5) * dt * data.gyro, Jr2);
d_next_d_gyro->template block<3, 3>(3, 0) =
next_state.T_w_i.so3().matrix() * Jr * dt;
d_next_d_gyro->template block<3, 3>(6, 0) =
SO3::hat(-accel_world * dt) * RR_w_i_new_2 * Jr2 * Scalar(0.5) * dt;
d_next_d_gyro->template block<3, 3>(0, 0) =
Scalar(0.5) * dt * d_next_d_gyro->template block<3, 3>(6, 0);
}
}
/// @brief Default constructor.
IntegratedImuMeasurement() noexcept {
cov_.setZero();
d_state_d_ba_.setZero();
d_state_d_bg_.setZero();
bias_gyro_lin_.setZero();
bias_accel_lin_.setZero();
}
/// @brief Constructor with start time and bias estimates.
IntegratedImuMeasurement(int64_t start_t_ns, const Vec3& bias_gyro_lin,
const Vec3& bias_accel_lin) noexcept
: start_t_ns_(start_t_ns),
bias_gyro_lin_(bias_gyro_lin),
bias_accel_lin_(bias_accel_lin) {
cov_.setZero();
d_state_d_ba_.setZero();
d_state_d_bg_.setZero();
}
/// @brief Integrate IMU data
///
/// @param[in] data IMU data
/// @param[in] accel_cov diagonal of accelerometer noise covariance matrix
/// @param[in] gyro_cov diagonal of gyroscope noise covariance matrix
void integrate(const ImuData<Scalar>& data, const Vec3& accel_cov,
const Vec3& gyro_cov) {
ImuData<Scalar> data_corrected = data;
data_corrected.t_ns -= start_t_ns_;
data_corrected.accel -= bias_accel_lin_;
data_corrected.gyro -= bias_gyro_lin_;
PoseVelState<Scalar> new_state;
MatNN F;
MatN3 A;
MatN3 G;
propagateState(delta_state_, data_corrected, new_state, &F, &A, &G);
delta_state_ = new_state;
cov_ = F * cov_ * F.transpose() +
A * accel_cov.asDiagonal() * A.transpose() +
G * gyro_cov.asDiagonal() * G.transpose();
sqrt_cov_inv_computed_ = false;
d_state_d_ba_ = -A + F * d_state_d_ba_;
d_state_d_bg_ = -G + F * d_state_d_bg_;
}
/// @brief Predict state given this pseudo-measurement
///
/// @param[in] state0 current state
/// @param[in] g gravity vector
/// @param[out] state1 predicted state
void predictState(const PoseVelState<Scalar>& state0, const Vec3& g,
PoseVelState<Scalar>& state1) const {
Scalar dt = delta_state_.t_ns * Scalar(1e-9);
state1.T_w_i.so3() = state0.T_w_i.so3() * delta_state_.T_w_i.so3();
state1.vel_w_i =
state0.vel_w_i + g * dt + state0.T_w_i.so3() * delta_state_.vel_w_i;
state1.T_w_i.translation() =
state0.T_w_i.translation() + state0.vel_w_i * dt +
Scalar(0.5) * g * dt * dt +
state0.T_w_i.so3() * delta_state_.T_w_i.translation();
}
/// @brief Compute residual between two states given this pseudo-measurement
/// and optionally compute Jacobians.
///
/// @param[in] state0 initial state
/// @param[in] g gravity vector
/// @param[in] state1 next state
/// @param[in] curr_bg current estimate of gyroscope bias
/// @param[in] curr_ba current estimate of accelerometer bias
/// @param[out] d_res_d_state0 if not nullptr, Jacobian of the residual with
/// respect to state0
/// @param[out] d_res_d_state1 if not nullptr, Jacobian of the residual with
/// respect to state1
/// @param[out] d_res_d_bg if not nullptr, Jacobian of the residual with
/// respect to gyroscope bias
/// @param[out] d_res_d_ba if not nullptr, Jacobian of the residual with
/// respect to accelerometer bias
/// @return residual
VecN residual(const PoseVelState<Scalar>& state0, const Vec3& g,
const PoseVelState<Scalar>& state1, const Vec3& curr_bg,
const Vec3& curr_ba, MatNN* d_res_d_state0 = nullptr,
MatNN* d_res_d_state1 = nullptr, MatN3* d_res_d_bg = nullptr,
MatN3* d_res_d_ba = nullptr) const {
Scalar dt = delta_state_.t_ns * Scalar(1e-9);
VecN res;
VecN bg_diff;
VecN ba_diff;
bg_diff = d_state_d_bg_ * (curr_bg - bias_gyro_lin_);
ba_diff = d_state_d_ba_ * (curr_ba - bias_accel_lin_);
BASALT_ASSERT(ba_diff.template segment<3>(3).isApproxToConstant(0));
Mat3 R0_inv = state0.T_w_i.so3().inverse().matrix();
Vec3 tmp =
R0_inv * (state1.T_w_i.translation() - state0.T_w_i.translation() -
state0.vel_w_i * dt - Scalar(0.5) * g * dt * dt);
res.template segment<3>(0) =
tmp - (delta_state_.T_w_i.translation() +
bg_diff.template segment<3>(0) + ba_diff.template segment<3>(0));
res.template segment<3>(3) =
(SO3::exp(bg_diff.template segment<3>(3)) * delta_state_.T_w_i.so3() *
state1.T_w_i.so3().inverse() * state0.T_w_i.so3())
.log();
Vec3 tmp2 = R0_inv * (state1.vel_w_i - state0.vel_w_i - g * dt);
res.template segment<3>(6) =
tmp2 - (delta_state_.vel_w_i + bg_diff.template segment<3>(6) +
ba_diff.template segment<3>(6));
if (d_res_d_state0 || d_res_d_state1) {
Mat3 J;
Sophus::rightJacobianInvSO3(res.template segment<3>(3), J);
if (d_res_d_state0) {
d_res_d_state0->setZero();
d_res_d_state0->template block<3, 3>(0, 0) = -R0_inv;
d_res_d_state0->template block<3, 3>(0, 3) = SO3::hat(tmp) * R0_inv;
d_res_d_state0->template block<3, 3>(3, 3) = J * R0_inv;
d_res_d_state0->template block<3, 3>(6, 3) = SO3::hat(tmp2) * R0_inv;
d_res_d_state0->template block<3, 3>(0, 6) = -R0_inv * dt;
d_res_d_state0->template block<3, 3>(6, 6) = -R0_inv;
}
if (d_res_d_state1) {
d_res_d_state1->setZero();
d_res_d_state1->template block<3, 3>(0, 0) = R0_inv;
d_res_d_state1->template block<3, 3>(3, 3) = -J * R0_inv;
d_res_d_state1->template block<3, 3>(6, 6) = R0_inv;
}
}
if (d_res_d_ba) {
*d_res_d_ba = -d_state_d_ba_;
}
if (d_res_d_bg) {
d_res_d_bg->setZero();
*d_res_d_bg = -d_state_d_bg_;
Mat3 J;
Sophus::leftJacobianInvSO3(res.template segment<3>(3), J);
d_res_d_bg->template block<3, 3>(3, 0) =
J * d_state_d_bg_.template block<3, 3>(3, 0);
}
return res;
}
/// @brief Time duretion of preintegrated measurement in nanoseconds.
int64_t get_dt_ns() const { return delta_state_.t_ns; }
/// @brief Start time of preintegrated measurement in nanoseconds.
int64_t get_start_t_ns() const { return start_t_ns_; }
/// @brief Inverse of the measurement covariance matrix
inline MatNN get_cov_inv() const {
if (!sqrt_cov_inv_computed_) {
compute_sqrt_cov_inv();
sqrt_cov_inv_computed_ = true;
}
return sqrt_cov_inv_.transpose() * sqrt_cov_inv_;
}
/// @brief Square root inverse of the measurement covariance matrix
inline const MatNN& get_sqrt_cov_inv() const {
if (!sqrt_cov_inv_computed_) {
compute_sqrt_cov_inv();
sqrt_cov_inv_computed_ = true;
}
return sqrt_cov_inv_;
}
/// @brief Measurement covariance matrix
const MatNN& get_cov() const { return cov_; }
// Just for testing...
/// @brief Delta state
const PoseVelState<Scalar>& getDeltaState() const { return delta_state_; }
/// @brief Jacobian of delta state with respect to accelerometer bias
const MatN3& get_d_state_d_ba() const { return d_state_d_ba_; }
/// @brief Jacobian of delta state with respect to gyroscope bias
const MatN3& get_d_state_d_bg() const { return d_state_d_bg_; }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/// @brief Helper function to compute square root of the inverse covariance
void compute_sqrt_cov_inv() const {
sqrt_cov_inv_.setIdentity();
auto ldlt = cov_.ldlt();
sqrt_cov_inv_ = ldlt.transpositionsP() * sqrt_cov_inv_;
ldlt.matrixL().solveInPlace(sqrt_cov_inv_);
VecN D_inv_sqrt;
for (size_t i = 0; i < POSE_VEL_SIZE; i++) {
if (ldlt.vectorD()[i] < std::numeric_limits<Scalar>::min()) {
D_inv_sqrt[i] = 0;
} else {
D_inv_sqrt[i] = Scalar(1.0) / sqrt(ldlt.vectorD()[i]);
}
}
sqrt_cov_inv_ = D_inv_sqrt.asDiagonal() * sqrt_cov_inv_;
}
int64_t start_t_ns_{0}; ///< Integration start time in nanoseconds
PoseVelState<Scalar> delta_state_; ///< Delta state
MatNN cov_; ///< Measurement covariance
mutable MatNN
sqrt_cov_inv_; ///< Cached square root inverse of measurement covariance
mutable bool sqrt_cov_inv_computed_{
false}; ///< If the cached square root inverse
///< covariance is computed
MatN3 d_state_d_ba_, d_state_d_bg_;
Vec3 bias_gyro_lin_, bias_accel_lin_;
};
} // namespace basalt

211
thirdparty/basalt-headers/include/basalt/serialization/eigen_io.h vendored Normal file
View File

@@ -0,0 +1,211 @@
/**
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 Serialization for Eigen and Sophus types
*/
#pragma once
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <sophus/sim3.hpp>
#include <cereal/archives/binary.hpp>
#include <cereal/archives/json.hpp>
namespace cereal {
// NOTE: Serialization functions for non-basalt types (for now Eigen and Sophus)
// are provided in a separate header to make them available to other libraries
// depending on basalt-headers. Beware that it is not possible to have different
// and incompatible definitions for these same types in other libraries or
// executables that also include basalt-headers, as this would lead to undefined
// behaviour. See
// https://groups.google.com/d/topic/cerealcpp/WswQi_Sh-bw/discussion for a more
// detailed discussion and possible workarounds.
// For binary-archives, don't save a size tag for compact representation.
template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options,
int _MaxRows, int _MaxCols>
std::enable_if_t<(_Rows > 0) && (_Cols > 0) &&
!traits::is_text_archive<Archive>::value>
serialize(
Archive& archive,
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m) {
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < _Cols; j++) {
archive(m(i, j));
}
}
// Note: if we can break binary compatibility, we might want to consider the
// following. However, this would only work for compact Scalar types that can
// be serialized / deserialized by memcopy, such as double.
// archive(binary_data(m.data(), _Rows * _Cols * sizeof(_Scalar)));
}
// For text-archives, save size-tag even for constant size matrices, to ensure
// that the serialization is more compact (e.g. for JSON with size tag is uses a
// simple array, whereas without, it stores a list of pairs like ("value0", v0).
template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options,
int _MaxRows, int _MaxCols>
std::enable_if_t<(_Rows > 0) && (_Cols > 0) &&
traits::is_text_archive<Archive>::value>
serialize(
Archive& archive,
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m) {
size_type s = static_cast<size_type>(_Rows * _Cols);
archive(make_size_tag(s));
if (s != _Rows * _Cols) {
throw std::runtime_error("matrix has incorrect length");
}
for (size_t i = 0; i < _Rows; i++) {
for (size_t j = 0; j < _Cols; j++) {
archive(m(i, j));
}
}
}
template <class Archive, class _Scalar, int _Cols, int _Options, int _MaxRows,
int _MaxCols>
std::enable_if_t<(_Cols > 0), void> save(
Archive& archive, const Eigen::Matrix<_Scalar, Eigen::Dynamic, _Cols,
_Options, _MaxRows, _MaxCols>& m) {
archive(make_size_tag(static_cast<size_type>(m.size())));
for (int i = 0; i < m.rows(); i++) {
for (int j = 0; j < _Cols; j++) {
archive(m(i, j));
}
}
}
template <class Archive, class _Scalar, int _Cols, int _Options, int _MaxRows,
int _MaxCols>
std::enable_if_t<(_Cols > 0), void> load(
Archive& archive,
Eigen::Matrix<_Scalar, Eigen::Dynamic, _Cols, _Options, _MaxRows, _MaxCols>&
m) {
size_type size;
archive(make_size_tag(size));
m.resize(Eigen::Index(size) / _Cols, _Cols);
for (int i = 0; i < m.rows(); i++) {
for (int j = 0; j < _Cols; j++) {
archive(m(i, j));
}
}
}
template <class Archive, class _Scalar, int _Rows, int _Options, int _MaxRows,
int _MaxCols>
std::enable_if_t<(_Rows > 0), void> save(
Archive& archive, const Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic,
_Options, _MaxRows, _MaxCols>& m) {
archive(make_size_tag(static_cast<size_type>(m.size())));
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < m.cols(); j++) {
archive(m(i, j));
}
}
}
template <class Archive, class _Scalar, int _Rows, int _Options, int _MaxRows,
int _MaxCols>
std::enable_if_t<(_Rows > 0), void> load(
Archive& archive,
Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic, _Options, _MaxRows, _MaxCols>&
m) {
size_type size;
archive(make_size_tag(size));
m.resize(_Rows, Eigen::Index(size) / _Rows);
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < m.cols(); j++) {
archive(m(i, j));
}
}
}
template <class Archive, class _Scalar, int _Options, int _MaxRows,
int _MaxCols>
void save(Archive& archive,
const Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options,
_MaxRows, _MaxCols>& m) {
archive(make_size_tag(static_cast<size_type>(m.rows())));
archive(make_size_tag(static_cast<size_type>(m.cols())));
for (int i = 0; i < m.rows(); i++) {
for (int j = 0; j < m.cols(); j++) {
archive(m(i, j));
}
}
}
template <class Archive, class _Scalar, int _Options, int _MaxRows,
int _MaxCols>
void load(Archive& archive,
Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options,
_MaxRows, _MaxCols>& m) {
size_type rows;
size_type cols;
archive(make_size_tag(rows));
archive(make_size_tag(cols));
m.resize(rows, cols);
for (int i = 0; i < m.rows(); i++) {
for (int j = 0; j < m.cols(); j++) {
archive(m(i, j));
}
}
}
template <class Archive>
void serialize(Archive& ar, Sophus::SE3d& p) {
ar(cereal::make_nvp("px", p.translation()[0]),
cereal::make_nvp("py", p.translation()[1]),
cereal::make_nvp("pz", p.translation()[2]),
cereal::make_nvp("qx", p.so3().data()[0]),
cereal::make_nvp("qy", p.so3().data()[1]),
cereal::make_nvp("qz", p.so3().data()[2]),
cereal::make_nvp("qw", p.so3().data()[3]));
}
template <class Archive>
void serialize(Archive& ar, Sophus::Sim3d& p) {
ar(cereal::make_nvp("px", p.translation()[0]),
cereal::make_nvp("py", p.translation()[1]),
cereal::make_nvp("pz", p.translation()[2]),
cereal::make_nvp("qx", p.rxso3().data()[0]),
cereal::make_nvp("qy", p.rxso3().data()[1]),
cereal::make_nvp("qz", p.rxso3().data()[2]),
cereal::make_nvp("qw", p.rxso3().data()[3]));
}
} // namespace cereal

266
thirdparty/basalt-headers/include/basalt/serialization/headers_serialization.h vendored Normal file
View File

@@ -0,0 +1,266 @@
/**
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 Serialization for basalt types
*/
#pragma once
#include <basalt/serialization/eigen_io.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/camera/bal_camera.hpp>
#include <cereal/archives/binary.hpp>
#include <cereal/archives/json.hpp>
#include <cereal/types/deque.hpp>
#include <cereal/types/memory.hpp>
#include <cereal/types/string.hpp>
#include <cereal/types/vector.hpp>
namespace cereal {
template <class Archive, class Scalar>
inline void save(Archive& ar, const basalt::GenericCamera<Scalar>& cam) {
std::visit(
[&](const auto& cam) {
ar(cereal::make_nvp("camera_type", cam.getName()));
ar(cereal::make_nvp("intrinsics", cam));
},
cam.variant);
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::GenericCamera<Scalar>& cam) {
std::string cam_type;
ar(cereal::make_nvp("camera_type", cam_type));
cam = basalt::GenericCamera<Scalar>::fromString(cam_type);
std::visit([&](auto& cam) { ar(cereal::make_nvp("intrinsics", cam)); },
cam.variant);
}
template <class Archive, class Scalar>
inline void save(Archive& ar, const basalt::KannalaBrandtCamera4<Scalar>& cam) {
ar(cereal::make_nvp("fx", cam.getParam()[0]),
cereal::make_nvp("fy", cam.getParam()[1]),
cereal::make_nvp("cx", cam.getParam()[2]),
cereal::make_nvp("cy", cam.getParam()[3]),
cereal::make_nvp("k1", cam.getParam()[4]),
cereal::make_nvp("k2", cam.getParam()[5]),
cereal::make_nvp("k3", cam.getParam()[6]),
cereal::make_nvp("k4", cam.getParam()[7]));
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::KannalaBrandtCamera4<Scalar>& cam) {
Eigen::Matrix<Scalar, 8, 1> intr;
ar(cereal::make_nvp("fx", intr[0]), cereal::make_nvp("fy", intr[1]),
cereal::make_nvp("cx", intr[2]), cereal::make_nvp("cy", intr[3]),
cereal::make_nvp("k1", intr[4]), cereal::make_nvp("k2", intr[5]),
cereal::make_nvp("k3", intr[6]), cereal::make_nvp("k4", intr[7]));
cam = basalt::KannalaBrandtCamera4<Scalar>(intr);
}
template <class Archive, class Scalar>
inline void save(Archive& ar,
const basalt::ExtendedUnifiedCamera<Scalar>& cam) {
ar(cereal::make_nvp("fx", cam.getParam()[0]),
cereal::make_nvp("fy", cam.getParam()[1]),
cereal::make_nvp("cx", cam.getParam()[2]),
cereal::make_nvp("cy", cam.getParam()[3]),
cereal::make_nvp("alpha", cam.getParam()[4]),
cereal::make_nvp("beta", cam.getParam()[5]));
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::ExtendedUnifiedCamera<Scalar>& cam) {
Eigen::Matrix<Scalar, 6, 1> intr;
ar(cereal::make_nvp("fx", intr[0]), cereal::make_nvp("fy", intr[1]),
cereal::make_nvp("cx", intr[2]), cereal::make_nvp("cy", intr[3]),
cereal::make_nvp("alpha", intr[4]), cereal::make_nvp("beta", intr[5]));
cam = basalt::ExtendedUnifiedCamera<Scalar>(intr);
}
template <class Archive, class Scalar>
inline void save(Archive& ar, const basalt::UnifiedCamera<Scalar>& cam) {
ar(cereal::make_nvp("fx", cam.getParam()[0]),
cereal::make_nvp("fy", cam.getParam()[1]),
cereal::make_nvp("cx", cam.getParam()[2]),
cereal::make_nvp("cy", cam.getParam()[3]),
cereal::make_nvp("alpha", cam.getParam()[4]));
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::UnifiedCamera<Scalar>& cam) {
Eigen::Matrix<Scalar, 5, 1> intr;
ar(cereal::make_nvp("fx", intr[0]), cereal::make_nvp("fy", intr[1]),
cereal::make_nvp("cx", intr[2]), cereal::make_nvp("cy", intr[3]),
cereal::make_nvp("alpha", intr[4]));
cam = basalt::UnifiedCamera<Scalar>(intr);
}
template <class Archive, class Scalar>
inline void save(Archive& ar, const basalt::PinholeCamera<Scalar>& cam) {
ar(cereal::make_nvp("fx", cam.getParam()[0]),
cereal::make_nvp("fy", cam.getParam()[1]),
cereal::make_nvp("cx", cam.getParam()[2]),
cereal::make_nvp("cy", cam.getParam()[3]));
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::PinholeCamera<Scalar>& cam) {
Eigen::Matrix<Scalar, 4, 1> intr;
ar(cereal::make_nvp("fx", intr[0]), cereal::make_nvp("fy", intr[1]),
cereal::make_nvp("cx", intr[2]), cereal::make_nvp("cy", intr[3]));
cam = basalt::PinholeCamera<Scalar>(intr);
}
template <class Archive, class Scalar>
inline void save(Archive& ar, const basalt::DoubleSphereCamera<Scalar>& cam) {
ar(cereal::make_nvp("fx", cam.getParam()[0]),
cereal::make_nvp("fy", cam.getParam()[1]),
cereal::make_nvp("cx", cam.getParam()[2]),
cereal::make_nvp("cy", cam.getParam()[3]),
cereal::make_nvp("xi", cam.getParam()[4]),
cereal::make_nvp("alpha", cam.getParam()[5]));
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::DoubleSphereCamera<Scalar>& cam) {
Eigen::Matrix<Scalar, 6, 1> intr;
ar(cereal::make_nvp("fx", intr[0]), cereal::make_nvp("fy", intr[1]),
cereal::make_nvp("cx", intr[2]), cereal::make_nvp("cy", intr[3]),
cereal::make_nvp("xi", intr[4]), cereal::make_nvp("alpha", intr[5]));
cam = basalt::DoubleSphereCamera<Scalar>(intr);
}
template <class Archive, class Scalar>
inline void save(Archive& ar, const basalt::FovCamera<Scalar>& cam) {
ar(cereal::make_nvp("fx", cam.getParam()[0]),
cereal::make_nvp("fy", cam.getParam()[1]),
cereal::make_nvp("cx", cam.getParam()[2]),
cereal::make_nvp("cy", cam.getParam()[3]),
cereal::make_nvp("w", cam.getParam()[4]));
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::FovCamera<Scalar>& cam) {
Eigen::Matrix<Scalar, 5, 1> intr;
ar(cereal::make_nvp("fx", intr[0]), cereal::make_nvp("fy", intr[1]),
cereal::make_nvp("cx", intr[2]), cereal::make_nvp("cy", intr[3]),
cereal::make_nvp("w", intr[4]));
cam = basalt::FovCamera<Scalar>(intr);
}
template <class Archive, class Scalar>
inline void save(Archive& ar, const basalt::BalCamera<Scalar>& cam) {
ar(cereal::make_nvp("f", cam.getParam()[0]),
cereal::make_nvp("k1", cam.getParam()[1]),
cereal::make_nvp("k2", cam.getParam()[2]));
}
template <class Archive, class Scalar>
inline void load(Archive& ar, basalt::BalCamera<Scalar>& cam) {
Eigen::Matrix<Scalar, 3, 1> intr;
ar(cereal::make_nvp("f", intr[0]), cereal::make_nvp("k1", intr[1]),
cereal::make_nvp("k2", intr[2]));
cam = basalt::BalCamera<Scalar>(intr);
}
template <class Archive, class Scalar, int DIM, int ORDER>
inline void save(Archive& ar,
const basalt::RdSpline<DIM, ORDER, Scalar>& spline) {
ar(spline.minTimeNs());
ar(spline.getTimeIntervalNs());
ar(spline.getKnots());
}
template <class Archive, class Scalar, int DIM, int ORDER>
inline void load(Archive& ar, basalt::RdSpline<DIM, ORDER, Scalar>& spline) {
int64_t start_t_ns;
int64_t dt_ns;
Eigen::aligned_deque<Eigen::Matrix<Scalar, DIM, 1>> knots;
ar(start_t_ns);
ar(dt_ns);
ar(knots);
basalt::RdSpline<DIM, ORDER, Scalar> new_spline(dt_ns, start_t_ns);
for (const auto& k : knots) {
new_spline.knotsPushBack(k);
}
spline = new_spline;
}
template <class Archive, class Scalar>
inline void serialize(Archive& ar, basalt::Calibration<Scalar>& cam) {
ar(cereal::make_nvp("T_imu_cam", cam.T_i_c),
cereal::make_nvp("intrinsics", cam.intrinsics),
cereal::make_nvp("resolution", cam.resolution),
cereal::make_nvp("calib_accel_bias", cam.calib_accel_bias.getParam()),
cereal::make_nvp("calib_gyro_bias", cam.calib_gyro_bias.getParam()),
cereal::make_nvp("imu_update_rate", cam.imu_update_rate),
cereal::make_nvp("accel_noise_std", cam.accel_noise_std),
cereal::make_nvp("gyro_noise_std", cam.gyro_noise_std),
cereal::make_nvp("accel_bias_std", cam.accel_bias_std),
cereal::make_nvp("gyro_bias_std", cam.gyro_bias_std),
cereal::make_nvp("cam_time_offset_ns", cam.cam_time_offset_ns),
cereal::make_nvp("vignette", cam.vignette));
}
template <class Archive, class Scalar>
inline void serialize(Archive& ar, basalt::MocapCalibration<Scalar>& cam) {
ar(cereal::make_nvp("T_mocap_world", cam.T_moc_w),
cereal::make_nvp("T_imu_marker", cam.T_i_mark),
cereal::make_nvp("mocap_time_offset_ns", cam.mocap_time_offset_ns),
cereal::make_nvp("mocap_to_imu_offset_ns", cam.mocap_to_imu_offset_ns));
}
} // namespace cereal

115
thirdparty/basalt-headers/include/basalt/spline/ceres_local_param.hpp vendored Normal file
View File

@@ -0,0 +1,115 @@
/**
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 Generic local parametrization for Sophus Lie group types to be used with
ceres.
*/
/**
File adapted from Sophus
Copyright 2011-2017 Hauke Strasdat
2012-2017 Steven Lovegrove
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to
deal in the Software without restriction, including without limitation the
rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
sell copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
IN THE SOFTWARE.
*/
#pragma once
#include <ceres/local_parameterization.h>
#include <sophus/se3.hpp>
namespace basalt {
/// @brief Local parametrization for ceres that can be used with Sophus Lie
/// group implementations.
template <class Groupd>
class LieLocalParameterization : public ceres::LocalParameterization {
public:
virtual ~LieLocalParameterization() {}
using Tangentd = typename Groupd::Tangent;
/// @brief plus operation for Ceres
///
/// T * exp(x)
///
virtual bool Plus(double const* T_raw, double const* delta_raw,
double* T_plus_delta_raw) const {
Eigen::Map<Groupd const> const T(T_raw);
Eigen::Map<Tangentd const> const delta(delta_raw);
Eigen::Map<Groupd> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * Groupd::exp(delta);
return true;
}
///@brief Jacobian of plus operation for Ceres
///
/// Dx T * exp(x) with x=0
///
virtual bool ComputeJacobian(double const* T_raw,
double* jacobian_raw) const {
Eigen::Map<Groupd const> T(T_raw);
Eigen::Map<Eigen::Matrix<double, Groupd::num_parameters, Groupd::DoF,
Eigen::RowMajor>>
jacobian(jacobian_raw);
jacobian = T.Dx_this_mul_exp_x_at_0();
return true;
}
///@brief Global size
virtual int GlobalSize() const { return Groupd::num_parameters; }
///@brief Local size
virtual int LocalSize() const { return Groupd::DoF; }
};
} // namespace basalt

236
thirdparty/basalt-headers/include/basalt/spline/ceres_spline_helper.h vendored Normal file
View File

@@ -0,0 +1,236 @@
/**
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 Helper for implementing Lie group and Euclidean b-splines in ceres.
*/
#pragma once
#include <basalt/spline/spline_common.h>
#include <Eigen/Dense>
namespace basalt {
/// @brief Helper for implementing Lie group and Euclidean b-splines in ceres of
/// order N
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _N>
struct CeresSplineHelper {
static constexpr int N = _N; // Order of the spline.
static constexpr int DEG = _N - 1; // Degree of the spline.
using MatN = Eigen::Matrix<double, _N, _N>;
using VecN = Eigen::Matrix<double, _N, 1>;
static const MatN blending_matrix_;
static const MatN cumulative_blending_matrix_;
static const MatN base_coefficients_;
/// @brief Vector of derivatives of time polynomial.
///
/// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots &
/// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first
/// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1)
/// t^{N-2}\end{bmatrix} \f$.
/// @param Derivative derivative to evaluate
/// @param[out] res_const vector to store the result
/// @param[in] t
template <int Derivative, class Derived>
static inline void baseCoeffsWithTime(
const Eigen::MatrixBase<Derived>& res_const, double t) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N);
Eigen::MatrixBase<Derived>& res =
const_cast<Eigen::MatrixBase<Derived>&>(res_const);
res.setZero();
if (Derivative < N) {
res[Derivative] = base_coefficients_(Derivative, Derivative);
double _t = t;
for (int j = Derivative + 1; j < N; j++) {
res[j] = base_coefficients_(Derivative, j) * _t;
_t = _t * t;
}
}
}
/// @brief Evaluate Lie group cummulative B-spline and time derivatives.
///
/// @param[in] sKnots array of pointers of the spline knots. The size of each
/// knot should be GroupT::num_parameters: 4 for SO(3) and 7 for SE(3).
/// @param[in] u normalized time to compute value of the spline
/// @param[in] inv_dt inverse of the time spacing in seconds between spline
/// knots
/// @param[out] transform_out if not nullptr return the value of the spline
/// @param[out] vel_out if not nullptr velocity (first time derivative) in the
/// body frame
/// @param[out] accel_out if not nullptr acceleration (second time derivative)
/// in the body frame
template <class T, template <class> class GroupT>
static inline void evaluate_lie(
T const* const* sKnots, const double u, const double inv_dt,
GroupT<T>* transform_out = nullptr,
typename GroupT<T>::Tangent* vel_out = nullptr,
typename GroupT<T>::Tangent* accel_out = nullptr,
typename GroupT<T>::Tangent* jerk_out = nullptr) {
using Group = GroupT<T>;
using Tangent = typename GroupT<T>::Tangent;
using Adjoint = typename GroupT<T>::Adjoint;
VecN p, coeff, dcoeff, ddcoeff, dddcoeff;
CeresSplineHelper<N>::template baseCoeffsWithTime<0>(p, u);
coeff = CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
if (vel_out || accel_out || jerk_out) {
CeresSplineHelper<N>::template baseCoeffsWithTime<1>(p, u);
dcoeff = inv_dt * CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
if (accel_out || jerk_out) {
CeresSplineHelper<N>::template baseCoeffsWithTime<2>(p, u);
ddcoeff = inv_dt * inv_dt *
CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
if (jerk_out) {
CeresSplineHelper<N>::template baseCoeffsWithTime<3>(p, u);
dddcoeff = inv_dt * inv_dt * inv_dt *
CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
}
}
}
if (transform_out) {
Eigen::Map<Group const> const p00(sKnots[0]);
*transform_out = p00;
}
Tangent rot_vel, rot_accel, rot_jerk;
if (vel_out || accel_out || jerk_out) rot_vel.setZero();
if (accel_out || jerk_out) rot_accel.setZero();
if (jerk_out) rot_jerk.setZero();
for (int i = 0; i < DEG; i++) {
Eigen::Map<Group const> const p0(sKnots[i]);
Eigen::Map<Group const> const p1(sKnots[i + 1]);
Group r01 = p0.inverse() * p1;
Tangent delta = r01.log();
Group exp_kdelta = Group::exp(delta * coeff[i + 1]);
if (transform_out) (*transform_out) *= exp_kdelta;
if (vel_out || accel_out || jerk_out) {
Adjoint A = exp_kdelta.inverse().Adj();
rot_vel = A * rot_vel;
Tangent rot_vel_current = delta * dcoeff[i + 1];
rot_vel += rot_vel_current;
if (accel_out || jerk_out) {
rot_accel = A * rot_accel;
Tangent accel_lie_bracket =
Group::lieBracket(rot_vel, rot_vel_current);
rot_accel += ddcoeff[i + 1] * delta + accel_lie_bracket;
if (jerk_out) {
rot_jerk = A * rot_jerk;
rot_jerk += dddcoeff[i + 1] * delta +
Group::lieBracket(ddcoeff[i + 1] * rot_vel +
2 * dcoeff[i + 1] * rot_accel -
dcoeff[i + 1] * accel_lie_bracket,
delta);
}
}
}
}
if (vel_out) *vel_out = rot_vel;
if (accel_out) *accel_out = rot_accel;
if (jerk_out) *jerk_out = rot_jerk;
}
/// @brief Evaluate Euclidean B-spline or time derivatives.
///
/// @param[in] sKnots array of pointers of the spline knots. The size of each
/// knot should be DIM.
/// @param[in] u normalized time to compute value of the spline
/// @param[in] inv_dt inverse of the time spacing in seconds between spline
/// knots
/// @param[out] vec_out if DERIV=0 returns value of the spline, otherwise
/// corresponding derivative.
template <class T, int DIM, int DERIV>
static inline void evaluate(T const* const* sKnots, const double u,
const double inv_dt,
Eigen::Matrix<T, DIM, 1>* vec_out) {
if (!vec_out) return;
using VecD = Eigen::Matrix<T, DIM, 1>;
VecN p, coeff;
CeresSplineHelper<N>::template baseCoeffsWithTime<DERIV>(p, u);
coeff =
std::pow(inv_dt, DERIV) * CeresSplineHelper<N>::blending_matrix_ * p;
vec_out->setZero();
for (int i = 0; i < N; i++) {
Eigen::Map<VecD const> const p(sKnots[i]);
(*vec_out) += coeff[i] * p;
}
}
};
template <int _N>
const typename CeresSplineHelper<_N>::MatN
CeresSplineHelper<_N>::base_coefficients_ =
basalt::computeBaseCoefficients<_N, double>();
template <int _N>
const typename CeresSplineHelper<_N>::MatN
CeresSplineHelper<_N>::blending_matrix_ =
basalt::computeBlendingMatrix<_N, double, false>();
template <int _N>
const typename CeresSplineHelper<_N>::MatN
CeresSplineHelper<_N>::cumulative_blending_matrix_ =
basalt::computeBlendingMatrix<_N, double, true>();
} // namespace basalt

363
thirdparty/basalt-headers/include/basalt/spline/rd_spline.h vendored Normal file
View File

@@ -0,0 +1,363 @@
/**
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 Uniform B-spline for euclidean vectors
*/
#pragma once
#include <basalt/spline/spline_common.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/sophus_utils.hpp>
#include <Eigen/Dense>
#include <array>
namespace basalt {
/// @brief Uniform B-spline for euclidean vectors with dimention DIM of order
/// N
///
/// For example, in the particular case scalar values and order N=5, for a time
/// \f$t \in [t_i, t_{i+1})\f$ the value of \f$p(t)\f$ depends only on 5 control
/// points at \f$[t_i, t_{i+1}, t_{i+2}, t_{i+3}, t_{i+4}]\f$. To
/// simplify calculations we transform time to uniform representation \f$s(t) =
/// (t - t_0)/\Delta t \f$, such that control points transform into \f$ s_i \in
/// [0,..,N] \f$. We define function \f$ u(t) = s(t)-s_i \f$ to be a time since
/// the start of the segment. Following the matrix representation of De Boor -
/// Cox formula, the value of the function can be
/// evaluated as follows: \f{align}{
/// p(u(t)) &=
/// \begin{pmatrix} p_{i}\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\\ p_{i+4}
/// \end{pmatrix}^T M_5 \begin{pmatrix} 1 \\ u \\ u^2 \\ u^3 \\ u^4
/// \end{pmatrix},
/// \f}
/// where \f$ p_{i} \f$ are knots and \f$ M_5 \f$ is a blending matrix computed
/// using \ref computeBlendingMatrix \f{align}{
/// M_5 = \frac{1}{4!}
/// \begin{pmatrix} 1 & -4 & 6 & -4 & 1 \\ 11 & -12 & -6 & 12 & -4 \\11 &
/// 12 & -6 & -12 & 6 \\ 1 & 4 & 6 & 4 & -4 \\ 0 & 0 & 0 & 0
/// & 1 \end{pmatrix}.
/// \f}
/// Given this formula, we can evaluate derivatives with respect to time
/// (velocity, acceleration) in the following way:
/// \f{align}{
/// p'(u(t)) &= \frac{1}{\Delta t}
/// \begin{pmatrix} p_{i}\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\\ p_{i+4}
/// \end{pmatrix}^T
/// M_5
/// \begin{pmatrix} 0 \\ 1 \\ 2u \\ 3u^2 \\ 4u^3 \end{pmatrix},
/// \f}
/// \f{align}{
/// p''(u(t)) &= \frac{1}{\Delta t^2}
/// \begin{pmatrix} p_{i}\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\\ p_{i+4}
/// \end{pmatrix}^T
/// M_5
/// \begin{pmatrix} 0 \\ 0 \\ 2 \\ 6u \\ 12u^2 \end{pmatrix}.
/// \f}
/// Higher time derivatives are evaluated similarly. This class supports
/// vector values for knots \f$ p_{i} \f$. The corresponding derivative vector
/// on the right is computed using \ref baseCoeffsWithTime.
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _DIM, int _N, typename _Scalar = double>
class RdSpline {
public:
static constexpr int N = _N; ///< Order of the spline.
static constexpr int DEG = _N - 1; ///< Degree of the spline.
static constexpr int DIM = _DIM; ///< Dimension of euclidean vector space.
static constexpr _Scalar NS_TO_S = 1e-9; ///< Nanosecond to second conversion
static constexpr _Scalar S_TO_NS = 1e9; ///< Second to nanosecond conversion
using MatN = Eigen::Matrix<_Scalar, _N, _N>;
using VecN = Eigen::Matrix<_Scalar, _N, 1>;
using VecD = Eigen::Matrix<_Scalar, _DIM, 1>;
using MatD = Eigen::Matrix<_Scalar, _DIM, _DIM>;
/// @brief Struct to store the Jacobian of the spline
///
/// Since B-spline of order N has local support (only N knots infuence the
/// value) the Jacobian is zero for all knots except maximum N for value and
/// all derivatives.
struct JacobianStruct {
size_t
start_idx; ///< Start index of the non-zero elements of the Jacobian.
std::array<_Scalar, N> d_val_d_knot; ///< Value of nonzero Jacobians.
};
/// @brief Default constructor
RdSpline() = default;
/// @brief Constructor with knot interval and start time
///
/// @param[in] time_interval_ns knot time interval in nanoseconds
/// @param[in] start_time_ns start time of the spline in nanoseconds
RdSpline(int64_t time_interval_ns, int64_t start_time_ns = 0)
: dt_ns_(time_interval_ns), start_t_ns_(start_time_ns) {
pow_inv_dt_[0] = 1.0;
pow_inv_dt_[1] = S_TO_NS / dt_ns_;
for (size_t i = 2; i < N; i++) {
pow_inv_dt_[i] = pow_inv_dt_[i - 1] * pow_inv_dt_[1];
}
}
/// @brief Cast to different scalar type
template <typename Scalar2>
inline RdSpline<_DIM, _N, Scalar2> cast() const {
RdSpline<_DIM, _N, Scalar2> res;
res.dt_ns_ = dt_ns_;
res.start_t_ns_ = start_t_ns_;
for (int i = 0; i < _N; i++) {
res.pow_inv_dt_[i] = pow_inv_dt_[i];
}
for (const auto& k : knots_) {
res.knots_.emplace_back(k.template cast<Scalar2>());
}
return res;
}
/// @brief Set start time for spline
///
/// @param[in] start_time_ns start time of the spline in nanoseconds
inline void setStartTimeNs(int64_t start_time_ns) {
start_t_ns_ = start_time_ns;
}
/// @brief Maximum time represented by spline
///
/// @return maximum time represented by spline in nanoseconds
int64_t maxTimeNs() const {
return start_t_ns_ + (knots_.size() - N + 1) * dt_ns_ - 1;
}
/// @brief Minimum time represented by spline
///
/// @return minimum time represented by spline in nanoseconds
int64_t minTimeNs() const { return start_t_ns_; }
/// @brief Gererate random trajectory
///
/// @param[in] n number of knots to generate
/// @param[in] static_init if true the first N knots will be the same
/// resulting in static initial condition
void genRandomTrajectory(int n, bool static_init = false) {
if (static_init) {
VecD rnd = VecD::Random() * 5;
for (int i = 0; i < N; i++) {
knots_.push_back(rnd);
}
for (int i = 0; i < n - N; i++) {
knots_.push_back(VecD::Random() * 5);
}
} else {
for (int i = 0; i < n; i++) {
knots_.push_back(VecD::Random() * 5);
}
}
}
/// @brief Add knot to the end of the spline
///
/// @param[in] knot knot to add
inline void knotsPushBack(const VecD& knot) { knots_.push_back(knot); }
/// @brief Remove knot from the back of the spline
inline void knotsPopBack() { knots_.pop_back(); }
/// @brief Return the first knot of the spline
///
/// @return first knot of the spline
inline const VecD& knotsFront() const { return knots_.front(); }
/// @brief Remove first knot of the spline and increase the start time
inline void knotsPopFront() {
start_t_ns_ += dt_ns_;
knots_.pop_front();
}
/// @brief Resize containter with knots
///
/// @param[in] n number of knots
inline void resize(size_t n) { knots_.resize(n); }
/// @brief Return reference to the knot with index i
///
/// @param i index of the knot
/// @return reference to the knot
inline VecD& getKnot(int i) { return knots_[i]; }
/// @brief Return const reference to the knot with index i
///
/// @param i index of the knot
/// @return const reference to the knot
inline const VecD& getKnot(int i) const { return knots_[i]; }
/// @brief Return const reference to deque with knots
///
/// @return const reference to deque with knots
const Eigen::aligned_deque<VecD>& getKnots() const { return knots_; }
/// @brief Return time interval in nanoseconds
///
/// @return time interval in nanoseconds
int64_t getTimeIntervalNs() const { return dt_ns_; }
/// @brief Evaluate value or derivative of the spline
///
/// @param Derivative derivative to evaluate (0 for value)
/// @param[in] time_ns time for evaluating of the spline in nanoseconds
/// @param[out] J if not nullptr, return the Jacobian of the value with
/// respect to knots
/// @return value of the spline or derivative. Euclidean vector of dimention
/// DIM.
template <int Derivative = 0>
VecD evaluate(int64_t time_ns, JacobianStruct* J = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<Derivative>(p, u);
VecN coeff = pow_inv_dt_[Derivative] * (BLENDING_MATRIX * p);
// std::cerr << "p " << p.transpose() << std::endl;
// std::cerr << "coeff " << coeff.transpose() << std::endl;
VecD res;
res.setZero();
for (int i = 0; i < N; i++) {
res += coeff[i] * knots_[s + i];
if (J) {
J->d_val_d_knot[i] = coeff[i];
}
}
if (J) {
J->start_idx = s;
}
return res;
}
/// @brief Alias for first derivative of spline. See \ref evaluate.
inline VecD velocity(int64_t time_ns, JacobianStruct* J = nullptr) const {
return evaluate<1>(time_ns, J);
}
/// @brief Alias for second derivative of spline. See \ref evaluate.
inline VecD acceleration(int64_t time_ns, JacobianStruct* J = nullptr) const {
return evaluate<2>(time_ns, J);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
/// @brief Vector of derivatives of time polynomial.
///
/// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots &
/// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first
/// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1)
/// t^{N-2}\end{bmatrix} \f$.
/// @param Derivative derivative to evaluate
/// @param[out] res_const vector to store the result
/// @param[in] t
template <int Derivative, class Derived>
static void baseCoeffsWithTime(const Eigen::MatrixBase<Derived>& res_const,
_Scalar t) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N);
Eigen::MatrixBase<Derived>& res =
const_cast<Eigen::MatrixBase<Derived>&>(res_const);
res.setZero();
if (Derivative < N) {
res[Derivative] = BASE_COEFFICIENTS(Derivative, Derivative);
_Scalar ti = t;
for (int j = Derivative + 1; j < N; j++) {
res[j] = BASE_COEFFICIENTS(Derivative, j) * ti;
ti = ti * t;
}
}
}
template <int, int, typename>
friend class RdSpline;
static const MatN
BLENDING_MATRIX; ///< Blending matrix. See \ref computeBlendingMatrix.
static const MatN BASE_COEFFICIENTS; ///< Base coefficients matrix.
///< See \ref computeBaseCoefficients.
Eigen::aligned_deque<VecD> knots_; ///< Knots
int64_t dt_ns_{0}; ///< Knot interval in nanoseconds
int64_t start_t_ns_{0}; ///< Start time in nanoseconds
std::array<_Scalar, _N> pow_inv_dt_; ///< Array with inverse powers of dt
};
template <int _DIM, int _N, typename _Scalar>
const typename RdSpline<_DIM, _N, _Scalar>::MatN
RdSpline<_DIM, _N, _Scalar>::BASE_COEFFICIENTS =
computeBaseCoefficients<_N, _Scalar>();
template <int _DIM, int _N, typename _Scalar>
const typename RdSpline<_DIM, _N, _Scalar>::MatN
RdSpline<_DIM, _N, _Scalar>::BLENDING_MATRIX =
computeBlendingMatrix<_N, _Scalar, false>();
} // namespace basalt

552
thirdparty/basalt-headers/include/basalt/spline/se3_spline.h vendored Normal file
View File

@@ -0,0 +1,552 @@
/**
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 Uniform B-spline for SE(3)
*/
#pragma once
#include <basalt/spline/rd_spline.h>
#include <basalt/spline/so3_spline.h>
#include <basalt/utils/assert.h>
#include <basalt/calibration/calib_bias.hpp>
#include <array>
namespace basalt {
/// @brief Uniform B-spline for SE(3) of order N. Internally uses an SO(3) (\ref
/// So3Spline) spline for rotation and 3D Euclidean spline (\ref RdSpline) for
/// translation (split representaion).
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _N, typename _Scalar = double>
class Se3Spline {
public:
static constexpr int N = _N; ///< Order of the spline.
static constexpr int DEG = _N - 1; ///< Degree of the spline.
using MatN = Eigen::Matrix<_Scalar, _N, _N>;
using VecN = Eigen::Matrix<_Scalar, _N, 1>;
using VecNp1 = Eigen::Matrix<_Scalar, _N + 1, 1>;
using Vec3 = Eigen::Matrix<_Scalar, 3, 1>;
using Vec6 = Eigen::Matrix<_Scalar, 6, 1>;
using Vec9 = Eigen::Matrix<_Scalar, 9, 1>;
using Vec12 = Eigen::Matrix<_Scalar, 12, 1>;
using Mat3 = Eigen::Matrix<_Scalar, 3, 3>;
using Mat6 = Eigen::Matrix<_Scalar, 6, 6>;
using Mat36 = Eigen::Matrix<_Scalar, 3, 6>;
using Mat39 = Eigen::Matrix<_Scalar, 3, 9>;
using Mat312 = Eigen::Matrix<_Scalar, 3, 12>;
using Matrix3Array = std::array<Mat3, N>;
using Matrix36Array = std::array<Mat36, N>;
using Matrix6Array = std::array<Mat6, N>;
using SO3 = Sophus::SO3<_Scalar>;
using SE3 = Sophus::SE3<_Scalar>;
using PosJacobianStruct = typename RdSpline<3, N, _Scalar>::JacobianStruct;
using SO3JacobianStruct = typename So3Spline<N, _Scalar>::JacobianStruct;
/// @brief Struct to store the accelerometer residual Jacobian with
/// respect to knots
struct AccelPosSO3JacobianStruct {
size_t start_idx;
std::array<Mat36, N> d_val_d_knot;
};
/// @brief Struct to store the pose Jacobian with respect to knots
struct PosePosSO3JacobianStruct {
size_t start_idx;
std::array<Mat6, N> d_val_d_knot;
};
/// @brief Constructor with knot interval and start time
///
/// @param[in] time_interval_ns knot time interval in nanoseconds
/// @param[in] start_time_ns start time of the spline in nanoseconds
Se3Spline(int64_t time_interval_ns, int64_t start_time_ns = 0)
: pos_spline_(time_interval_ns, start_time_ns),
so3_spline_(time_interval_ns, start_time_ns),
dt_ns_(time_interval_ns) {}
/// @brief Gererate random trajectory
///
/// @param[in] n number of knots to generate
/// @param[in] static_init if true the first N knots will be the same
/// resulting in static initial condition
void genRandomTrajectory(int n, bool static_init = false) {
so3_spline_.genRandomTrajectory(n, static_init);
pos_spline_.genRandomTrajectory(n, static_init);
}
/// @brief Set the knot to particular SE(3) pose
///
/// @param[in] pose SE(3) pose
/// @param[in] i index of the knot
void setKnot(const Sophus::SE3d &pose, int i) {
so3_spline_.getKnot(i) = pose.so3();
pos_spline_.getKnot(i) = pose.translation();
}
/// @brief Reset spline to have num_knots initialized at pose
///
/// @param[in] pose SE(3) pose
/// @param[in] num_knots number of knots to initialize
void setKnots(const Sophus::SE3d &pose, int num_knots) {
so3_spline_.resize(num_knots);
pos_spline_.resize(num_knots);
for (int i = 0; i < num_knots; i++) {
so3_spline_.getKnot(i) = pose.so3();
pos_spline_.getKnot(i) = pose.translation();
}
}
/// @brief Reset spline to the knots from other spline
///
/// @param[in] other spline to copy knots from
void setKnots(const Se3Spline<N, _Scalar> &other) {
BASALT_ASSERT(other.dt_ns_ == dt_ns_);
BASALT_ASSERT(other.pos_spline_.getKnots().size() ==
other.pos_spline_.getKnots().size());
size_t num_knots = other.pos_spline_.getKnots().size();
so3_spline_.resize(num_knots);
pos_spline_.resize(num_knots);
for (size_t i = 0; i < num_knots; i++) {
so3_spline_.getKnot(i) = other.so3_spline_.getKnot(i);
pos_spline_.getKnot(i) = other.pos_spline_.getKnot(i);
}
}
/// @brief Add knot to the end of the spline
///
/// @param[in] knot knot to add
inline void knotsPushBack(const SE3 &knot) {
so3_spline_.knotsPushBack(knot.so3());
pos_spline_.knotsPushBack(knot.translation());
}
/// @brief Remove knot from the back of the spline
inline void knotsPopBack() {
so3_spline_.knotsPopBack();
pos_spline_.knotsPopBack();
}
/// @brief Return the first knot of the spline
///
/// @return first knot of the spline
inline SE3 knotsFront() const {
SE3 res(so3_spline_.knots_front(), pos_spline_.knots_front());
return res;
}
/// @brief Remove first knot of the spline and increase the start time
inline void knotsPopFront() {
so3_spline_.knots_pop_front();
pos_spline_.knots_pop_front();
BASALT_ASSERT(so3_spline_.minTimeNs() == pos_spline_.minTimeNs());
BASALT_ASSERT(so3_spline_.getKnots().size() ==
pos_spline_.getKnots().size());
}
/// @brief Return the last knot of the spline
///
/// @return last knot of the spline
SE3 getLastKnot() {
BASALT_ASSERT(so3_spline_.getKnots().size() ==
pos_spline_.getKnots().size());
SE3 res(so3_spline_.getKnots().back(), pos_spline_.getKnots().back());
return res;
}
/// @brief Return knot with index i
///
/// @param i index of the knot
/// @return knot
SE3 getKnot(size_t i) const {
SE3 res(getKnotSO3(i), getKnotPos(i));
return res;
}
/// @brief Return reference to the SO(3) knot with index i
///
/// @param i index of the knot
/// @return reference to the SO(3) knot
inline SO3 &getKnotSO3(size_t i) { return so3_spline_.getKnot(i); }
/// @brief Return const reference to the SO(3) knot with index i
///
/// @param i index of the knot
/// @return const reference to the SO(3) knot
inline const SO3 &getKnotSO3(size_t i) const {
return so3_spline_.getKnot(i);
}
/// @brief Return reference to the position knot with index i
///
/// @param i index of the knot
/// @return reference to the position knot
inline Vec3 &getKnotPos(size_t i) { return pos_spline_.getKnot(i); }
/// @brief Return const reference to the position knot with index i
///
/// @param i index of the knot
/// @return const reference to the position knot
inline const Vec3 &getKnotPos(size_t i) const {
return pos_spline_.getKnot(i);
}
/// @brief Set start time for spline
///
/// @param[in] start_time_ns start time of the spline in nanoseconds
inline void setStartTimeNs(int64_t s) {
so3_spline_.setStartTimeNs(s);
pos_spline_.setStartTimeNs(s);
}
/// @brief Apply increment to the knot
///
/// The incremernt vector consists of translational and rotational parts \f$
/// [\upsilon, \omega]^T \f$. Given the current pose of the knot \f$ R \in
/// SO(3), p \in \mathbb{R}^3\f$ the updated pose is: \f{align}{ R' &=
/// \exp(\omega) R
/// \\ p' &= p + \upsilon
/// \f}
/// The increment is consistent with \ref
/// PoseState::applyInc.
///
/// @param[in] i index of the knot
/// @param[in] inc 6x1 increment vector
template <typename Derived>
void applyInc(int i, const Eigen::MatrixBase<Derived> &inc) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 6);
pos_spline_.getKnot(i) += inc.template head<3>();
so3_spline_.getKnot(i) =
SO3::exp(inc.template tail<3>()) * so3_spline_.getKnot(i);
}
/// @brief Maximum time represented by spline
///
/// @return maximum time represented by spline in nanoseconds
int64_t maxTimeNs() const {
BASALT_ASSERT_STREAM(so3_spline_.maxTimeNs() == pos_spline_.maxTimeNs(),
"so3_spline.maxTimeNs() " << so3_spline_.maxTimeNs()
<< " pos_spline.maxTimeNs() "
<< pos_spline_.maxTimeNs());
return pos_spline_.maxTimeNs();
}
/// @brief Minimum time represented by spline
///
/// @return minimum time represented by spline in nanoseconds
int64_t minTimeNs() const {
BASALT_ASSERT_STREAM(so3_spline_.minTimeNs() == pos_spline_.minTimeNs(),
"so3_spline.minTimeNs() " << so3_spline_.minTimeNs()
<< " pos_spline.minTimeNs() "
<< pos_spline_.minTimeNs());
return pos_spline_.minTimeNs();
}
/// @brief Number of knots in the spline
size_t numKnots() const { return pos_spline_.getKnots().size(); }
/// @brief Linear acceleration in the world frame.
///
/// @param[in] time_ns time to evaluate linear acceleration in nanoseconds
inline Vec3 transAccelWorld(int64_t time_ns) const {
return pos_spline_.acceleration(time_ns);
}
/// @brief Linear velocity in the world frame.
///
/// @param[in] time_ns time to evaluate linear velocity in nanoseconds
inline Vec3 transVelWorld(int64_t time_ns) const {
return pos_spline_.velocity(time_ns);
}
/// @brief Rotational velocity in the body frame.
///
/// @param[in] time_ns time to evaluate rotational velocity in nanoseconds
inline Vec3 rotVelBody(int64_t time_ns) const {
return so3_spline_.velocityBody(time_ns);
}
/// @brief Evaluate pose.
///
/// @param[in] time_ns time to evaluate pose in nanoseconds
/// @return SE(3) pose at time_ns
SE3 pose(int64_t time_ns) const {
SE3 res;
res.so3() = so3_spline_.evaluate(time_ns);
res.translation() = pos_spline_.evaluate(time_ns);
return res;
}
/// @brief Evaluate pose and compute Jacobian.
///
/// @param[in] time_ns time to evaluate pose in nanoseconds
/// @param[out] J Jacobian of the pose with respect to knots
/// @return SE(3) pose at time_ns
Sophus::SE3d pose(int64_t time_ns, PosePosSO3JacobianStruct *J) const {
Sophus::SE3d res;
typename So3Spline<_N, _Scalar>::JacobianStruct Jr;
typename RdSpline<3, N, _Scalar>::JacobianStruct Jp;
res.so3() = so3_spline_.evaluate(time_ns, &Jr);
res.translation() = pos_spline_.evaluate(time_ns, &Jp);
if (J) {
Eigen::Matrix3d RT = res.so3().inverse().matrix();
J->start_idx = Jr.start_idx;
for (int i = 0; i < N; i++) {
J->d_val_d_knot[i].setZero();
J->d_val_d_knot[i].template topLeftCorner<3, 3>() =
RT * Jp.d_val_d_knot[i];
J->d_val_d_knot[i].template bottomRightCorner<3, 3>() =
RT * Jr.d_val_d_knot[i];
}
}
return res;
}
/// @brief Evaluate pose and compute time Jacobian.
///
/// @param[in] time_ns time to evaluate pose in nanoseconds
/// @param[out] J Jacobian of the pose with time
void d_pose_d_t(int64_t time_ns, Vec6 &J) const {
J.template head<3>() =
so3_spline_.evaluate(time_ns).inverse() * transVelWorld(time_ns);
J.template tail<3>() = rotVelBody(time_ns);
}
/// @brief Evaluate gyroscope residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement gyroscope measurement
/// @param[in] gyro_bias_full gyroscope calibration
/// @return gyroscope residual
Vec3 gyroResidual(int64_t time_ns, const Vec3 &measurement,
const CalibGyroBias<_Scalar> &gyro_bias_full) const {
return so3_spline_.velocityBody(time_ns) -
gyro_bias_full.getCalibrated(measurement);
}
/// @brief Evaluate gyroscope residual and compute Jacobians.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement gyroscope measurement
/// @param[in] gyro_bias_full gyroscope calibration
/// @param[out] J_knots Jacobian with respect to SO(3) spline knots
/// @param[out] J_bias Jacobian with respect to gyroscope calibration
/// @return gyroscope residual
Vec3 gyroResidual(int64_t time_ns, const Vec3 &measurement,
const CalibGyroBias<_Scalar> &gyro_bias_full,
SO3JacobianStruct *J_knots,
Mat312 *J_bias = nullptr) const {
if (J_bias) {
J_bias->setZero();
J_bias->template block<3, 3>(0, 0).diagonal().array() = 1.0;
J_bias->template block<3, 3>(0, 3).diagonal().array() = -measurement[0];
J_bias->template block<3, 3>(0, 6).diagonal().array() = -measurement[1];
J_bias->template block<3, 3>(0, 9).diagonal().array() = -measurement[2];
}
return so3_spline_.velocityBody(time_ns, J_knots) -
gyro_bias_full.getCalibrated(measurement);
}
/// @brief Evaluate accelerometer residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement accelerometer measurement
/// @param[in] accel_bias_full accelerometer calibration
/// @param[in] g gravity
/// @return accelerometer residual
Vec3 accelResidual(int64_t time_ns, const Eigen::Vector3d &measurement,
const CalibAccelBias<_Scalar> &accel_bias_full,
const Eigen::Vector3d &g) const {
Sophus::SO3d R = so3_spline_.evaluate(time_ns);
Eigen::Vector3d accel_world = pos_spline_.acceleration(time_ns);
return R.inverse() * (accel_world + g) -
accel_bias_full.getCalibrated(measurement);
}
/// @brief Evaluate accelerometer residual and Jacobians.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement accelerometer measurement
/// @param[in] accel_bias_full accelerometer calibration
/// @param[in] g gravity
/// @param[out] J_knots Jacobian with respect to spline knots
/// @param[out] J_bias Jacobian with respect to accelerometer calibration
/// @param[out] J_g Jacobian with respect to gravity
/// @return accelerometer residual
Vec3 accelResidual(int64_t time_ns, const Vec3 &measurement,
const CalibAccelBias<_Scalar> &accel_bias_full,
const Vec3 &g, AccelPosSO3JacobianStruct *J_knots,
Mat39 *J_bias = nullptr, Mat3 *J_g = nullptr) const {
typename So3Spline<_N, _Scalar>::JacobianStruct Jr;
typename RdSpline<3, N, _Scalar>::JacobianStruct Jp;
Sophus::SO3d R = so3_spline_.evaluate(time_ns, &Jr);
Eigen::Vector3d accel_world = pos_spline_.acceleration(time_ns, &Jp);
Eigen::Matrix3d RT = R.inverse().matrix();
Eigen::Matrix3d tmp = RT * Sophus::SO3d::hat(accel_world + g);
BASALT_ASSERT_STREAM(
Jr.start_idx == Jp.start_idx,
"Jr.start_idx " << Jr.start_idx << " Jp.start_idx " << Jp.start_idx);
BASALT_ASSERT_STREAM(
so3_spline_.getKnots().size() == pos_spline_.getKnots().size(),
"so3_spline.getKnots().size() " << so3_spline_.getKnots().size()
<< " pos_spline.getKnots().size() "
<< pos_spline_.getKnots().size());
J_knots->start_idx = Jp.start_idx;
for (int i = 0; i < N; i++) {
J_knots->d_val_d_knot[i].template topLeftCorner<3, 3>() =
RT * Jp.d_val_d_knot[i];
J_knots->d_val_d_knot[i].template bottomRightCorner<3, 3>() =
tmp * Jr.d_val_d_knot[i];
}
if (J_bias) {
J_bias->setZero();
J_bias->template block<3, 3>(0, 0).diagonal().array() = 1.0;
J_bias->template block<3, 3>(0, 3).diagonal().array() = -measurement[0];
(*J_bias)(1, 6) = -measurement[1];
(*J_bias)(2, 7) = -measurement[1];
(*J_bias)(2, 8) = -measurement[2];
}
if (J_g) {
(*J_g) = RT;
}
Vec3 res =
RT * (accel_world + g) - accel_bias_full.getCalibrated(measurement);
return res;
}
/// @brief Evaluate position residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measured_position position measurement
/// @param[out] Jp if not nullptr, Jacobian with respect to knos of the
/// position spline
/// @return position residual
Sophus::Vector3d positionResidual(int64_t time_ns,
const Vec3 &measured_position,
PosJacobianStruct *Jp = nullptr) const {
return pos_spline_.evaluate(time_ns, Jp) - measured_position;
}
/// @brief Evaluate orientation residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measured_orientation orientation measurement
/// @param[out] Jr if not nullptr, Jacobian with respect to knos of the
/// SO(3) spline
/// @return orientation residual
Sophus::Vector3d orientationResidual(int64_t time_ns,
const SO3 &measured_orientation,
SO3JacobianStruct *Jr = nullptr) const {
Sophus::Vector3d res =
(so3_spline_.evaluate(time_ns, Jr) * measured_orientation.inverse())
.log();
if (Jr) {
Eigen::Matrix3d Jrot;
Sophus::leftJacobianSO3(res, Jrot);
for (int i = 0; i < N; i++) {
Jr->d_val_d_knot[i] = Jrot * Jr->d_val_d_knot[i];
}
}
return res;
}
/// @brief Print knots for debugging.
inline void printKnots() const {
for (size_t i = 0; i < pos_spline_.getKnots().size(); i++) {
std::cout << i << ": p:" << pos_spline_.getKnot(i).transpose() << " q: "
<< so3_spline_.getKnot(i).unit_quaternion().coeffs().transpose()
<< std::endl;
}
}
/// @brief Print position knots for debugging.
inline void printPosKnots() const {
for (size_t i = 0; i < pos_spline_.getKnots().size(); i++) {
std::cout << pos_spline_.getKnot(i).transpose() << std::endl;
}
}
/// @brief Knot time interval in nanoseconds.
inline int64_t getDtNs() const { return dt_ns_; }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
RdSpline<3, _N, _Scalar> pos_spline_; ///< Position spline
So3Spline<_N, _Scalar> so3_spline_; ///< Orientation spline
int64_t dt_ns_; ///< Knot interval in nanoseconds
};
} // namespace basalt

790
thirdparty/basalt-headers/include/basalt/spline/so3_spline.h vendored Normal file
View File

@@ -0,0 +1,790 @@
/**
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 Uniform cumulative B-spline for SO(3)
*/
#pragma once
#include <basalt/spline/spline_common.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/sophus_utils.hpp>
#include <Eigen/Dense>
#include <sophus/so3.hpp>
#include <array>
namespace basalt {
/// @brief Uniform cummulative B-spline for SO(3) of order N
///
/// For example, in the particular case scalar values and order N=5, for a time
/// \f$t \in [t_i, t_{i+1})\f$ the value of \f$p(t)\f$ depends only on 5 control
/// points at \f$[t_i, t_{i+1}, t_{i+2}, t_{i+3}, t_{i+4}]\f$. To
/// simplify calculations we transform time to uniform representation \f$s(t) =
/// (t - t_0)/\Delta t \f$, such that control points transform into \f$ s_i \in
/// [0,..,N] \f$. We define function \f$ u(t) = s(t)-s_i \f$ to be a time since
/// the start of the segment. Following the cummulative matrix representation of
/// De Boor - Cox formula, the value of the function can be evaluated as
/// follows: \f{align}{
/// R(u(t)) &= R_i
/// \prod_{j=1}^{4}{\exp(k_{j}\log{(R_{i+j-1}^{-1}R_{i+j})})},
/// \\ \begin{pmatrix} k_0 \\ k_1 \\ k_2 \\ k_3 \\ k_4 \end{pmatrix}^T &=
/// M_{c5} \begin{pmatrix} 1 \\ u \\ u^2 \\ u^3 \\ u^4
/// \end{pmatrix},
/// \f}
/// where \f$ R_{i} \in SO(3) \f$ are knots and \f$ M_{c5} \f$ is a cummulative
/// blending matrix computed using \ref computeBlendingMatrix \f{align}{
/// M_{c5} = \frac{1}{4!}
/// \begin{pmatrix} 24 & 0 & 0 & 0 & 0 \\ 23 & 4 & -6 & 4 & -1 \\ 12 & 16 & 0
/// & -8 & 3 \\ 1 & 4 & 6 & 4 & -3 \\ 0 & 0 & 0 & 0 & 1 \end{pmatrix}.
/// \f}
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _N, typename _Scalar = double>
class So3Spline {
public:
static constexpr int N = _N; ///< Order of the spline.
static constexpr int DEG = _N - 1; ///< Degree of the spline.
static constexpr _Scalar NS_TO_S = 1e-9; ///< Nanosecond to second conversion
static constexpr _Scalar S_TO_NS = 1e9; ///< Second to nanosecond conversion
using MatN = Eigen::Matrix<_Scalar, _N, _N>;
using VecN = Eigen::Matrix<_Scalar, _N, 1>;
using Vec3 = Eigen::Matrix<_Scalar, 3, 1>;
using Mat3 = Eigen::Matrix<_Scalar, 3, 3>;
using SO3 = Sophus::SO3<_Scalar>;
/// @brief Struct to store the Jacobian of the spline
///
/// Since B-spline of order N has local support (only N knots infuence the
/// value) the Jacobian is zero for all knots except maximum N for value and
/// all derivatives.
struct JacobianStruct {
size_t start_idx;
std::array<Mat3, _N> d_val_d_knot;
};
/// @brief Constructor with knot interval and start time
///
/// @param[in] time_interval_ns knot time interval in nanoseconds
/// @param[in] start_time_ns start time of the spline in nanoseconds
So3Spline(int64_t time_interval_ns, int64_t start_time_ns = 0)
: dt_ns_(time_interval_ns), start_t_ns_(start_time_ns) {
pow_inv_dt_[0] = 1.0;
pow_inv_dt_[1] = S_TO_NS / dt_ns_;
pow_inv_dt_[2] = pow_inv_dt_[1] * pow_inv_dt_[1];
pow_inv_dt_[3] = pow_inv_dt_[2] * pow_inv_dt_[1];
}
/// @brief Maximum time represented by spline
///
/// @return maximum time represented by spline in nanoseconds
int64_t maxTimeNs() const {
return start_t_ns_ + (knots_.size() - N + 1) * dt_ns_ - 1;
}
/// @brief Minimum time represented by spline
///
/// @return minimum time represented by spline in nanoseconds
int64_t minTimeNs() const { return start_t_ns_; }
/// @brief Gererate random trajectory
///
/// @param[in] n number of knots to generate
/// @param[in] static_init if true the first N knots will be the same
/// resulting in static initial condition
void genRandomTrajectory(int n, bool static_init = false) {
if (static_init) {
Vec3 rnd = Vec3::Random() * M_PI;
for (int i = 0; i < N; i++) {
knots_.push_back(SO3::exp(rnd));
}
for (int i = 0; i < n - N; i++) {
knots_.push_back(knots_.back() * SO3::exp(Vec3::Random() * M_PI / 2));
}
} else {
knots_.push_back(SO3::exp(Vec3::Random() * M_PI));
for (int i = 1; i < n; i++) {
knots_.push_back(knots_.back() * SO3::exp(Vec3::Random() * M_PI / 2));
}
}
}
/// @brief Set start time for spline
///
/// @param[in] start_time_ns start time of the spline in nanoseconds
inline void setStartTimeNs(int64_t s) { start_t_ns_ = s; }
/// @brief Add knot to the end of the spline
///
/// @param[in] knot knot to add
inline void knotsPushBack(const SO3& knot) { knots_.push_back(knot); }
/// @brief Remove knot from the back of the spline
inline void knotsPopBack() { knots_.pop_back(); }
/// @brief Return the first knot of the spline
///
/// @return first knot of the spline
inline const SO3& knotsFront() const { return knots_.front(); }
/// @brief Remove first knot of the spline and increase the start time
inline void knotsPopFront() {
start_t_ns_ += dt_ns_;
knots_.pop_front();
}
/// @brief Resize containter with knots
///
/// @param[in] n number of knots
inline void resize(size_t n) { knots_.resize(n); }
/// @brief Return reference to the knot with index i
///
/// @param i index of the knot
/// @return reference to the knot
inline SO3& getKnot(int i) { return knots_[i]; }
/// @brief Return const reference to the knot with index i
///
/// @param i index of the knot
/// @return const reference to the knot
inline const SO3& getKnot(int i) const { return knots_[i]; }
/// @brief Return const reference to deque with knots
///
/// @return const reference to deque with knots
const Eigen::aligned_deque<SO3>& getKnots() const { return knots_; }
/// @brief Return time interval in nanoseconds
///
/// @return time interval in nanoseconds
int64_t getTimeIntervalNs() const { return dt_ns_; }
/// @brief Evaluate SO(3) B-spline
///
/// @param[in] time_ns time for evaluating the value of the spline in
/// nanoseconds
/// @param[out] J if not nullptr, return the Jacobian of the value with
/// respect to knots
/// @return SO(3) value of the spline
SO3 evaluate(int64_t time_ns, JacobianStruct* J = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
SO3 res = knots_[s];
Mat3 J_helper;
if (J) {
J->start_idx = s;
J_helper.setIdentity();
}
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
Vec3 kdelta = delta * coeff[i + 1];
if (J) {
Mat3 Jl_inv_delta;
Mat3 Jl_k_delta;
Sophus::leftJacobianInvSO3(delta, Jl_inv_delta);
Sophus::leftJacobianSO3(kdelta, Jl_k_delta);
J->d_val_d_knot[i] = J_helper;
J_helper = coeff[i + 1] * res.matrix() * Jl_k_delta * Jl_inv_delta *
p0.inverse().matrix();
J->d_val_d_knot[i] -= J_helper;
}
res *= SO3::exp(kdelta);
}
if (J) {
J->d_val_d_knot[DEG] = J_helper;
}
return res;
}
/// @brief Evaluate rotational velocity (first time derivative) of SO(3)
/// B-spline in the body frame
/// First, let's note that for scalars \f$ k, \Delta k \f$ the following
/// holds: \f$ \exp((k+\Delta k)\phi) = \exp(k\phi)\exp(\Delta k\phi), \phi
/// \in \mathbb{R}^3\f$. This is due to the fact that rotations around the
/// same axis are commutative.
///
/// Let's take SO(3) B-spline with N=3 as an example. The evolution in time of
/// rotation from the body frame to the world frame is described with \f[
/// R_{wb}(t) = R(t) = R_i \exp(k_1(t) \log(R_{i}^{-1}R_{i+1})) \exp(k_2(t)
/// \log(R_{i+1}^{-1}R_{i+2})), \f] where \f$ k_1, k_2 \f$ are spline
/// coefficients (see detailed description of \ref So3Spline). Since
/// expressions under logmap do not depend on time we can rename them to
/// constants.
/// \f[ R(t) = R_i \exp(k_1(t) ~ d_1) \exp(k_2(t) ~ d_2). \f]
///
/// With linear approximation of the spline coefficient evolution over time
/// \f$ k_1(t_0 + \Delta t) = k_1(t_0) + k_1'(t_0)\Delta t \f$ we can write
/// \f{align}
/// R(t_0 + \Delta t) &= R_i \exp( (k_1(t_0) + k_1'(t_0) \Delta t) ~ d_1)
/// \exp((k_2(t_0) + k_2'(t_0) \Delta t) ~ d_2)
/// \\ &= R_i \exp(k_1(t_0) ~ d_1) \exp(k_1'(t_0)~ d_1 \Delta t )
/// \exp(k_2(t_0) ~ d_2) \exp(k_2'(t_0) ~ d_2 \Delta t )
/// \\ &= R_i \exp(k_1(t_0) ~ d_1)
/// \exp(k_2(t_0) ~ d_2) \exp(R_{a}^T k_1'(t_0)~ d_1 \Delta t )
/// \exp(k_2'(t_0) ~ d_2 \Delta t )
/// \\ &= R_i \exp(k_1(t_0) ~ d_1)
/// \exp(k_2(t_0) ~ d_2) \exp((R_{a}^T k_1'(t_0)~ d_1 +
/// k_2'(t_0) ~ d_2) \Delta t )
/// \\ &= R(t_0) \exp((R_{a}^T k_1'(t_0)~ d_1 +
/// k_2'(t_0) ~ d_2) \Delta t )
/// \\ &= R(t_0) \exp( \omega \Delta t ),
/// \f} where \f$ \Delta t \f$ is small, \f$ R_{a} \in SO(3) = \exp(k_2(t_0) ~
/// d_2) \f$ and \f$ \omega \f$ is the rotational velocity in the body frame.
/// More explicitly we have the formula for rotational velocity in the body
/// frame \f[ \omega = R_{a}^T k_1'(t_0)~ d_1 + k_2'(t_0) ~ d_2. \f]
/// Derivatives of spline coefficients can be computed with \ref
/// baseCoeffsWithTime similar to \ref RdSpline (detailed description). With
/// the recursive formula computations generalize to different orders of
/// spline N.
///
/// @param[in] time_ns time for evaluating velocity of the spline in
/// nanoseconds
/// @return rotational velocity (3x1 vector)
Vec3 velocityBody(int64_t time_ns) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
Vec3 rot_vel;
rot_vel.setZero();
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
rot_vel = SO3::exp(-delta * coeff[i + 1]) * rot_vel;
rot_vel += delta * dcoeff[i + 1];
}
return rot_vel;
}
/// @brief Evaluate rotational velocity (first time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating velocity of the spline in
/// nanoseconds
/// @param[out] J if not nullptr, return the Jacobian of the rotational
/// velocity in body frame with respect to knots
/// @return rotational velocity (3x1 vector)
Vec3 velocityBody(int64_t time_ns, JacobianStruct* J) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
Vec3 delta_vec[DEG];
Mat3 R_tmp[DEG];
SO3 accum;
SO3 exp_k_delta[DEG];
Mat3 Jr_delta_inv[DEG];
Mat3 Jr_kdelta[DEG];
for (int i = DEG - 1; i >= 0; i--) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
delta_vec[i] = r01.log();
Sophus::rightJacobianInvSO3(delta_vec[i], Jr_delta_inv[i]);
Jr_delta_inv[i] *= p1.inverse().matrix();
Vec3 k_delta = coeff[i + 1] * delta_vec[i];
Sophus::rightJacobianSO3(-k_delta, Jr_kdelta[i]);
R_tmp[i] = accum.matrix();
exp_k_delta[i] = Sophus::SO3d::exp(-k_delta);
accum *= exp_k_delta[i];
}
Mat3 d_vel_d_delta[DEG];
d_vel_d_delta[0] = dcoeff[1] * R_tmp[0] * Jr_delta_inv[0];
Vec3 rot_vel = delta_vec[0] * dcoeff[1];
for (int i = 1; i < DEG; i++) {
d_vel_d_delta[i] =
R_tmp[i - 1] * SO3::hat(rot_vel) * Jr_kdelta[i] * coeff[i + 1] +
R_tmp[i] * dcoeff[i + 1];
d_vel_d_delta[i] *= Jr_delta_inv[i];
rot_vel = exp_k_delta[i] * rot_vel + delta_vec[i] * dcoeff[i + 1];
}
if (J) {
J->start_idx = s;
for (int i = 0; i < N; i++) {
J->d_val_d_knot[i].setZero();
}
for (int i = 0; i < DEG; i++) {
J->d_val_d_knot[i] -= d_vel_d_delta[i];
J->d_val_d_knot[i + 1] += d_vel_d_delta[i];
}
}
return rot_vel;
}
/// @brief Evaluate rotational acceleration (second time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating acceleration of the spline in
/// nanoseconds
/// @param[out] vel_body if not nullptr, return the rotational velocity in the
/// body frame (3x1 vector) (side computation)
/// @return rotational acceleration (3x1 vector)
Vec3 accelerationBody(int64_t time_ns, Vec3* vel_body = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
baseCoeffsWithTime<2>(p, u);
VecN ddcoeff = pow_inv_dt_[2] * BLENDING_MATRIX * p;
SO3 r_accum;
Vec3 rot_vel;
rot_vel.setZero();
Vec3 rot_accel;
rot_accel.setZero();
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
SO3 rot = SO3::exp(-delta * coeff[i + 1]);
rot_vel = rot * rot_vel;
Vec3 vel_current = dcoeff[i + 1] * delta;
rot_vel += vel_current;
rot_accel = rot * rot_accel;
rot_accel += ddcoeff[i + 1] * delta + rot_vel.cross(vel_current);
}
if (vel_body) {
*vel_body = rot_vel;
}
return rot_accel;
}
/// @brief Evaluate rotational acceleration (second time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating acceleration of the spline in
/// nanoseconds
/// @param[out] J_accel if not nullptr, return the Jacobian of the rotational
/// acceleration in body frame with respect to knots
/// @param[out] vel_body if not nullptr, return the rotational velocity in the
/// body frame (3x1 vector) (side computation)
/// @param[out] J_vel if not nullptr, return the Jacobian of the rotational
/// velocity in the body frame (side computation)
/// @return rotational acceleration (3x1 vector)
Vec3 accelerationBody(int64_t time_ns, JacobianStruct* J_accel,
Vec3* vel_body = nullptr,
JacobianStruct* J_vel = nullptr) const {
BASALT_ASSERT(J_accel);
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
baseCoeffsWithTime<2>(p, u);
VecN ddcoeff = pow_inv_dt_[2] * BLENDING_MATRIX * p;
Vec3 delta_vec[DEG];
Mat3 exp_k_delta[DEG];
Mat3 Jr_delta_inv[DEG];
Mat3 Jr_kdelta[DEG];
Vec3 rot_vel;
rot_vel.setZero();
Vec3 rot_accel;
rot_accel.setZero();
Vec3 rot_vel_arr[DEG];
Vec3 rot_accel_arr[DEG];
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
delta_vec[i] = r01.log();
Sophus::rightJacobianInvSO3(delta_vec[i], Jr_delta_inv[i]);
Jr_delta_inv[i] *= p1.inverse().matrix();
Vec3 k_delta = coeff[i + 1] * delta_vec[i];
Sophus::rightJacobianSO3(-k_delta, Jr_kdelta[i]);
exp_k_delta[i] = Sophus::SO3d::exp(-k_delta).matrix();
rot_vel = exp_k_delta[i] * rot_vel;
Vec3 vel_current = dcoeff[i + 1] * delta_vec[i];
rot_vel += vel_current;
rot_accel = exp_k_delta[i] * rot_accel;
rot_accel += ddcoeff[i + 1] * delta_vec[i] + rot_vel.cross(vel_current);
rot_vel_arr[i] = rot_vel;
rot_accel_arr[i] = rot_accel;
}
Mat3 d_accel_d_delta[DEG];
Mat3 d_vel_d_delta[DEG];
d_vel_d_delta[DEG - 1] = coeff[DEG] * exp_k_delta[DEG - 1] *
SO3::hat(rot_vel_arr[DEG - 2]) *
Jr_kdelta[DEG - 1] +
Mat3::Identity() * dcoeff[DEG];
d_accel_d_delta[DEG - 1] =
coeff[DEG] * exp_k_delta[DEG - 1] * SO3::hat(rot_accel_arr[DEG - 2]) *
Jr_kdelta[DEG - 1] +
Mat3::Identity() * ddcoeff[DEG] +
dcoeff[DEG] * (SO3::hat(rot_vel_arr[DEG - 1]) -
SO3::hat(delta_vec[DEG - 1]) * d_vel_d_delta[DEG - 1]);
Mat3 pj;
pj.setIdentity();
Vec3 sj;
sj.setZero();
for (int i = DEG - 2; i >= 0; i--) {
sj += dcoeff[i + 2] * pj * delta_vec[i + 1];
pj *= exp_k_delta[i + 1];
d_vel_d_delta[i] = Mat3::Identity() * dcoeff[i + 1];
if (i >= 1) {
d_vel_d_delta[i] += coeff[i + 1] * exp_k_delta[i] *
SO3::hat(rot_vel_arr[i - 1]) * Jr_kdelta[i];
}
d_accel_d_delta[i] =
Mat3::Identity() * ddcoeff[i + 1] +
dcoeff[i + 1] * (SO3::hat(rot_vel_arr[i]) -
SO3::hat(delta_vec[i]) * d_vel_d_delta[i]);
if (i >= 1) {
d_accel_d_delta[i] += coeff[i + 1] * exp_k_delta[i] *
SO3::hat(rot_accel_arr[i - 1]) * Jr_kdelta[i];
}
d_vel_d_delta[i] = pj * d_vel_d_delta[i];
d_accel_d_delta[i] =
pj * d_accel_d_delta[i] - SO3::hat(sj) * d_vel_d_delta[i];
}
if (J_vel) {
J_vel->start_idx = s;
for (int i = 0; i < N; i++) {
J_vel->d_val_d_knot[i].setZero();
}
for (int i = 0; i < DEG; i++) {
Mat3 val = d_vel_d_delta[i] * Jr_delta_inv[i];
J_vel->d_val_d_knot[i] -= val;
J_vel->d_val_d_knot[i + 1] += val;
}
}
if (J_accel) {
J_accel->start_idx = s;
for (int i = 0; i < N; i++) {
J_accel->d_val_d_knot[i].setZero();
}
for (int i = 0; i < DEG; i++) {
Mat3 val = d_accel_d_delta[i] * Jr_delta_inv[i];
J_accel->d_val_d_knot[i] -= val;
J_accel->d_val_d_knot[i + 1] += val;
}
}
if (vel_body) {
*vel_body = rot_vel;
}
return rot_accel;
}
/// @brief Evaluate rotational jerk (third time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating jerk of the spline in
/// nanoseconds
/// @param[out] vel_body if not nullptr, return the rotational velocity in the
/// body frame (3x1 vector) (side computation)
/// @param[out] accel_body if not nullptr, return the rotational acceleration
/// in the body frame (3x1 vector) (side computation)
/// @return rotational jerk (3x1 vector)
Vec3 jerkBody(int64_t time_ns, Vec3* vel_body = nullptr,
Vec3* accel_body = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
baseCoeffsWithTime<2>(p, u);
VecN ddcoeff = pow_inv_dt_[2] * BLENDING_MATRIX * p;
baseCoeffsWithTime<3>(p, u);
VecN dddcoeff = pow_inv_dt_[3] * BLENDING_MATRIX * p;
Vec3 rot_vel;
rot_vel.setZero();
Vec3 rot_accel;
rot_accel.setZero();
Vec3 rot_jerk;
rot_jerk.setZero();
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
SO3 rot = SO3::exp(-delta * coeff[i + 1]);
rot_vel = rot * rot_vel;
Vec3 vel_current = dcoeff[i + 1] * delta;
rot_vel += vel_current;
rot_accel = rot * rot_accel;
Vec3 rot_vel_cross_vel_current = rot_vel.cross(vel_current);
rot_accel += ddcoeff[i + 1] * delta + rot_vel_cross_vel_current;
rot_jerk = rot * rot_jerk;
rot_jerk += dddcoeff[i + 1] * delta +
(ddcoeff[i + 1] * rot_vel + 2 * dcoeff[i + 1] * rot_accel -
dcoeff[i + 1] * rot_vel_cross_vel_current)
.cross(delta);
}
if (vel_body) {
*vel_body = rot_vel;
}
if (accel_body) {
*accel_body = rot_accel;
}
return rot_jerk;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
/// @brief Vector of derivatives of time polynomial.
///
/// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots &
/// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first
/// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1)
/// t^{N-2}\end{bmatrix} \f$.
/// @param Derivative derivative to evaluate
/// @param[out] res_const vector to store the result
/// @param[in] t
template <int Derivative, class Derived>
static void baseCoeffsWithTime(const Eigen::MatrixBase<Derived>& res_const,
_Scalar t) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N);
Eigen::MatrixBase<Derived>& res =
const_cast<Eigen::MatrixBase<Derived>&>(res_const);
res.setZero();
if (Derivative < N) {
res[Derivative] = BASE_COEFFICIENTS(Derivative, Derivative);
_Scalar ti = t;
for (int j = Derivative + 1; j < N; j++) {
res[j] = BASE_COEFFICIENTS(Derivative, j) * ti;
ti = ti * t;
}
}
}
static const MatN
BLENDING_MATRIX; ///< Blending matrix. See \ref computeBlendingMatrix.
static const MatN BASE_COEFFICIENTS; ///< Base coefficients matrix.
///< See \ref computeBaseCoefficients.
Eigen::aligned_deque<SO3> knots_; ///< Knots
int64_t dt_ns_; ///< Knot interval in nanoseconds
int64_t start_t_ns_; ///< Start time in nanoseconds
std::array<_Scalar, 4> pow_inv_dt_; ///< Array with inverse powers of dt
}; // namespace basalt
template <int _N, typename _Scalar>
const typename So3Spline<_N, _Scalar>::MatN
So3Spline<_N, _Scalar>::BASE_COEFFICIENTS =
computeBaseCoefficients<_N, _Scalar>();
template <int _N, typename _Scalar>
const typename So3Spline<_N, _Scalar>::MatN
So3Spline<_N, _Scalar>::BLENDING_MATRIX =
computeBlendingMatrix<_N, _Scalar, true>();
} // namespace basalt

137
thirdparty/basalt-headers/include/basalt/spline/spline_common.h vendored Normal file
View File

@@ -0,0 +1,137 @@
/**
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 Common functions for B-spline evaluation
*/
#pragma once
#include <Eigen/Dense>
#include <cstdint>
namespace basalt {
/// @brief Compute binomial coefficient.
///
/// Computes number of combinations that include k objects out of n.
/// @param[in] n
/// @param[in] k
/// @return binomial coefficient
constexpr inline uint64_t binomialCoefficient(uint64_t n, uint64_t k) {
if (k > n) {
return 0;
}
uint64_t r = 1;
for (uint64_t d = 1; d <= k; ++d) {
r *= n--;
r /= d;
}
return r;
}
/// @brief Compute blending matrix for uniform B-spline evaluation.
///
/// @param _N order of the spline
/// @param _Scalar scalar type to use
/// @param _Cumulative if the spline should be cumulative
template <int _N, typename _Scalar = double, bool _Cumulative = false>
Eigen::Matrix<_Scalar, _N, _N> computeBlendingMatrix() {
Eigen::Matrix<double, _N, _N> m;
m.setZero();
for (int i = 0; i < _N; ++i) {
for (int j = 0; j < _N; ++j) {
double sum = 0;
for (int s = j; s < _N; ++s) {
sum += std::pow(-1.0, s - j) * binomialCoefficient(_N, s - j) *
std::pow(_N - s - 1.0, _N - 1.0 - i);
}
m(j, i) = binomialCoefficient(_N - 1, _N - 1 - i) * sum;
}
}
if (_Cumulative) {
for (int i = 0; i < _N; i++) {
for (int j = i + 1; j < _N; j++) {
m.row(i) += m.row(j);
}
}
}
uint64_t factorial = 1;
for (int i = 2; i < _N; ++i) {
factorial *= i;
}
return (m / factorial).template cast<_Scalar>();
}
/// @brief Compute base coefficient matrix for polynomials of size N.
///
/// In each row starting from 0 contains the derivative coefficients of the
/// polynomial. For _N=5 we get the following matrix: \f[ \begin{bmatrix}
/// 1 & 1 & 1 & 1 & 1
/// \\0 & 1 & 2 & 3 & 4
/// \\0 & 0 & 2 & 6 & 12
/// \\0 & 0 & 0 & 6 & 24
/// \\0 & 0 & 0 & 0 & 24
/// \\ \end{bmatrix}
/// \f]
/// Functions \ref RdSpline::baseCoeffsWithTime and \ref
/// So3Spline::baseCoeffsWithTime use this matrix to compute derivatives of the
/// time polynomial.
///
/// @param _N order of the polynomial
/// @param _Scalar scalar type to use
template <int _N, typename _Scalar = double>
Eigen::Matrix<_Scalar, _N, _N> computeBaseCoefficients() {
Eigen::Matrix<double, _N, _N> base_coefficients;
base_coefficients.setZero();
base_coefficients.row(0).setOnes();
constexpr int DEG = _N - 1;
int order = DEG;
for (int n = 1; n < _N; n++) {
for (int i = DEG - order; i < _N; i++) {
base_coefficients(n, i) = (order - DEG + i) * base_coefficients(n - 1, i);
}
order--;
}
return base_coefficients.template cast<_Scalar>();
}
} // namespace basalt

126
thirdparty/basalt-headers/include/basalt/utils/assert.h vendored Normal file
View 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 Assertions used in the project
*/
#pragma once
#include <iostream>
namespace basalt {
#define UNUSED(x) (void)(x)
#define BASALT_ATTRIBUTE_NORETURN __attribute__((noreturn))
inline BASALT_ATTRIBUTE_NORETURN void assertionFailed(char const* expr,
char const* function,
char const* file,
long line) {
std::cerr << "***** Assertion (" << expr << ") failed in " << function
<< ":\n"
<< file << ':' << line << ":" << std::endl;
std::abort();
}
inline BASALT_ATTRIBUTE_NORETURN void assertionFailedMsg(char const* expr,
char const* msg,
char const* function,
char const* file,
long line) {
std::cerr << "***** Assertion (" << expr << ") failed in " << function
<< ":\n"
<< file << ':' << line << ": " << msg << std::endl;
std::abort();
}
inline BASALT_ATTRIBUTE_NORETURN void logFatal(char const* function,
char const* file, long line) {
std::cerr << "***** Fatal error in " << function << ":\n"
<< file << ':' << line << ":" << std::endl;
std::abort();
}
inline BASALT_ATTRIBUTE_NORETURN void logFatalMsg(char const* msg,
char const* function,
char const* file, long line) {
std::cerr << "***** Fatal error in " << function << ":\n"
<< file << ':' << line << ": " << msg << std::endl;
std::abort();
}
} // namespace basalt
#define BASALT_LIKELY(x) __builtin_expect(x, 1)
#if defined(BASALT_DISABLE_ASSERTS)
#define BASALT_ASSERT(expr) ((void)0)
#define BASALT_ASSERT_MSG(expr, msg) ((void)0)
#define BASALT_ASSERT_STREAM(expr, msg) ((void)0)
#else
#define BASALT_ASSERT(expr) \
(BASALT_LIKELY(!!(expr)) \
? ((void)0) \
: ::basalt::assertionFailed(#expr, __PRETTY_FUNCTION__, __FILE__, \
__LINE__))
#define BASALT_ASSERT_MSG(expr, msg) \
(BASALT_LIKELY(!!(expr)) \
? ((void)0) \
: ::basalt::assertionFailedMsg(#expr, msg, __PRETTY_FUNCTION__, \
__FILE__, __LINE__))
#define BASALT_ASSERT_STREAM(expr, msg) \
(BASALT_LIKELY(!!(expr)) \
? ((void)0) \
: (std::cerr << msg << std::endl, \
::basalt::assertionFailed(#expr, __PRETTY_FUNCTION__, __FILE__, \
__LINE__)))
#endif
#define BASALT_LOG_FATAL(msg) \
::basalt::logFatalMsg(msg, __PRETTY_FUNCTION__, __FILE__, __LINE__)
#define BASALT_LOG_FATAL_STREAM(msg) \
(std::cerr << msg << std::endl, \
::basalt::logFatal(__PRETTY_FUNCTION__, __FILE__, __LINE__))

65
thirdparty/basalt-headers/include/basalt/utils/eigen_utils.hpp vendored Normal file
View File

@@ -0,0 +1,65 @@
/**
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 Definition of the standard containers with Eigen allocator.
*/
#pragma once
#include <deque>
#include <map>
#include <unordered_map>
#include <vector>
#include <Eigen/Dense>
namespace Eigen {
template <typename T>
using aligned_vector = std::vector<T, Eigen::aligned_allocator<T>>;
template <typename T>
using aligned_deque = std::deque<T, Eigen::aligned_allocator<T>>;
template <typename K, typename V>
using aligned_map = std::map<K, V, std::less<K>,
Eigen::aligned_allocator<std::pair<K const, V>>>;
template <typename K, typename V>
using aligned_unordered_map =
std::unordered_map<K, V, std::hash<K>, std::equal_to<K>,
Eigen::aligned_allocator<std::pair<K const, V>>>;
} // namespace Eigen

26
thirdparty/basalt-headers/include/basalt/utils/hash.h vendored Normal file
View File

@@ -0,0 +1,26 @@
#pragma once
#include <cstddef>
namespace basalt {
// to work around static_assert(false, ...)
template <class T>
struct dependent_false : std::false_type {};
template <class T>
inline void hash_combine(std::size_t& seed, const T& value) {
// Simple hash_combine, see e.g. here:
// https://github.com/HowardHinnant/hash_append/issues/7
// Not sure we ever need 32bit, but here it is...
if constexpr (sizeof(std::size_t) == 4) {
seed ^= std::hash<T>{}(value) + 0x9e3779b9U + (seed << 6) + (seed >> 2);
} else if constexpr (sizeof(std::size_t) == 8) {
seed ^= std::hash<T>{}(value) + 0x9e3779b97f4a7c15LLU + (seed << 12) +
(seed >> 4);
} else {
static_assert(dependent_false<T>::value, "hash_combine not implemented");
}
}
} // namespace basalt

538
thirdparty/basalt-headers/include/basalt/utils/sophus_utils.hpp vendored Normal file
View File

@@ -0,0 +1,538 @@
/**
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 Useful utilities to work with SO(3) and SE(3) groups from Sophus.
*/
#pragma once
#include <sophus/se3.hpp>
#include <sophus/sim3.hpp>
#include <basalt/utils/assert.h>
#include <basalt/utils/eigen_utils.hpp>
namespace Sophus {
/// @brief Decoupled version of logmap for SE(3)
///
/// For SE(3) element vector
/// \f[
/// \begin{pmatrix} R & t \\ 0 & 1 \end{pmatrix} \in SE(3),
/// \f]
/// returns \f$ (t, \log(R)) \in \mathbb{R}^6 \f$. Here rotation is not coupled
/// with translation.
///
/// @param[in] SE(3) member
/// @return tangent vector (6x1 vector)
template <typename Scalar>
inline typename SE3<Scalar>::Tangent se3_logd(const SE3<Scalar> &se3) {
typename SE3<Scalar>::Tangent upsilon_omega;
upsilon_omega.template tail<3>() = se3.so3().log();
upsilon_omega.template head<3>() = se3.translation();
return upsilon_omega;
}
/// @brief Decoupled version of expmap for SE(3)
///
/// For tangent vector \f$ (\upsilon, \omega) \in \mathbb{R}^6 \f$ returns
/// \f[
/// \begin{pmatrix} \exp(\omega) & \upsilon \\ 0 & 1 \end{pmatrix} \in SE(3),
/// \f]
/// where \f$ \exp(\omega) \in SO(3) \f$. Here rotation is not coupled with
/// translation.
///
/// @param[in] tangent vector (6x1 vector)
/// @return SE(3) member
template <typename Derived>
inline SE3<typename Derived::Scalar> se3_expd(
const Eigen::MatrixBase<Derived> &upsilon_omega) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 6);
using Scalar = typename Derived::Scalar;
return SE3<Scalar>(SO3<Scalar>::exp(upsilon_omega.template tail<3>()),
upsilon_omega.template head<3>());
}
/// @brief Decoupled version of logmap for Sim(3)
///
/// For Sim(3) element vector
/// \f[
/// \begin{pmatrix} sR & t \\ 0 & 1 \end{pmatrix} \in SE(3),
/// \f]
/// returns \f$ (t, \log(R), log(s)) \in \mathbb{R}^3 \f$. Here rotation and
/// scale are not coupled with translation. Rotation and scale are commutative
/// anyway.
///
/// @param[in] Sim(3) member
/// @return tangent vector (7x1 vector)
template <typename Scalar>
inline typename Sim3<Scalar>::Tangent sim3_logd(const Sim3<Scalar> &sim3) {
typename Sim3<Scalar>::Tangent upsilon_omega_sigma;
upsilon_omega_sigma.template tail<4>() = sim3.rxso3().log();
upsilon_omega_sigma.template head<3>() = sim3.translation();
return upsilon_omega_sigma;
}
/// @brief Decoupled version of expmap for Sim(3)
///
/// For tangent vector \f$ (\upsilon, \omega, \sigma) \in \mathbb{R}^7 \f$
/// returns
/// \f[
/// \begin{pmatrix} \exp(\sigma)\exp(\omega) & \upsilon \\ 0 & 1 \end{pmatrix}
/// \in Sim(3),
/// \f]
/// where \f$ \exp(\omega) \in SO(3) \f$. Here rotation and scale are not
/// coupled with translation. Rotation and scale are commutative anyway.
///
/// @param[in] tangent vector (7x1 vector)
/// @return Sim(3) member
template <typename Derived>
inline Sim3<typename Derived::Scalar> sim3_expd(
const Eigen::MatrixBase<Derived> &upsilon_omega_sigma) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 7);
using Scalar = typename Derived::Scalar;
return Sim3<Scalar>(
RxSO3<Scalar>::exp(upsilon_omega_sigma.template tail<4>()),
upsilon_omega_sigma.template head<3>());
}
// Note on the use of const_cast in the following functions: The output
// parameter is only marked 'const' to make the C++ compiler accept a temporary
// expression here. These functions use const_cast it, so constness isn't
// honored here. See:
// https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
/// @brief Right Jacobian for SO(3)
///
/// For \f$ \exp(x) \in SO(3) \f$ provides a Jacobian that approximates the sum
/// under expmap with a right multiplication of expmap for small \f$ \epsilon
/// \f$. Can be used to compute: \f$ \exp(\phi + \epsilon) \approx \exp(\phi)
/// \exp(J_{\phi} \epsilon)\f$
/// @param[in] phi (3x1 vector)
/// @param[out] J_phi (3x3 matrix)
template <typename Derived1, typename Derived2>
inline void rightJacobianSO3(const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
Scalar phi_norm2 = phi.squaredNorm();
Eigen::Matrix<Scalar, 3, 3> phi_hat = Sophus::SO3<Scalar>::hat(phi);
Eigen::Matrix<Scalar, 3, 3> phi_hat2 = phi_hat * phi_hat;
J.setIdentity();
if (phi_norm2 > Sophus::Constants<Scalar>::epsilon()) {
Scalar phi_norm = std::sqrt(phi_norm2);
Scalar phi_norm3 = phi_norm2 * phi_norm;
J -= phi_hat * (1 - std::cos(phi_norm)) / phi_norm2;
J += phi_hat2 * (phi_norm - std::sin(phi_norm)) / phi_norm3;
} else {
// Taylor expansion around 0
J -= phi_hat / 2;
J += phi_hat2 / 6;
}
}
/// @brief Right Inverse Jacobian for SO(3)
///
/// For \f$ \exp(x) \in SO(3) \f$ provides an inverse Jacobian that approximates
/// the logmap of the right multiplication of expmap of the arguments with a sum
/// for small \f$ \epsilon \f$. Can be used to compute: \f$ \log
/// (\exp(\phi) \exp(\epsilon)) \approx \phi + J_{\phi} \epsilon\f$
/// @param[in] phi (3x1 vector)
/// @param[out] J_phi (3x3 matrix)
template <typename Derived1, typename Derived2>
inline void rightJacobianInvSO3(const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
Scalar phi_norm2 = phi.squaredNorm();
Eigen::Matrix<Scalar, 3, 3> phi_hat = Sophus::SO3<Scalar>::hat(phi);
Eigen::Matrix<Scalar, 3, 3> phi_hat2 = phi_hat * phi_hat;
J.setIdentity();
J += phi_hat / 2;
if (phi_norm2 > Sophus::Constants<Scalar>::epsilon()) {
Scalar phi_norm = std::sqrt(phi_norm2);
// We require that the angle is in range [0, pi]. We check if we are close
// to pi and apply a Taylor expansion to scalar multiplier of phi_hat2.
// Technically, log(exp(phi)exp(epsilon)) is not continuous / differentiable
// at phi=pi, but we still aim to return a reasonable value for all valid
// inputs.
BASALT_ASSERT(phi_norm <= M_PI + Sophus::Constants<Scalar>::epsilon());
if (phi_norm < M_PI - Sophus::Constants<Scalar>::epsilonSqrt()) {
// regular case for range (0,pi)
J += phi_hat2 * (1 / phi_norm2 - (1 + std::cos(phi_norm)) /
(2 * phi_norm * std::sin(phi_norm)));
} else {
// 0th-order Taylor expansion around pi
J += phi_hat2 / (M_PI * M_PI);
}
} else {
// Taylor expansion around 0
J += phi_hat2 / 12;
}
}
// Alternative version of rightJacobianInvSO3 that normalizes the angle to
// [0,pi]. However, it's too complicated and we decided to instead assert the
// input range, assuming that it is almost never really required (e.g. b/c the
// input is computed from Log()). Regardless, we leave this version here for
// future reference.
/*
template <typename Derived1, typename Derived2>
inline void rightJacobianInvSO3(const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
Scalar phi_norm2 = phi.squaredNorm();
if (phi_norm2 < Sophus::Constants<Scalar>::epsilon()) {
// short-circuit small angle case: Avoid computing sqrt(phi_norm2).
Eigen::Matrix<Scalar, 3, 3> phi_hat = Sophus::SO3<Scalar>::hat(phi);
Eigen::Matrix<Scalar, 3, 3> phi_hat2 = phi_hat * phi_hat;
J.setIdentity();
J += phi_hat / 2;
// Taylor expansion around 0
J += phi_hat2 / 12;
} else {
// non-small angle case: Compute angle.
Scalar phi_norm = std::sqrt(phi_norm2);
// Check phi_norm > pi case and compute phi_hat (we assume that we later
// don't use phi directly, and thus don't update it)
Eigen::Matrix<Scalar, 3, 3> phi_hat;
if (phi_norm > M_PI) {
// In the definition of the inverse Jacobian we consider the effect of a
// perturbation on exp(phi). So here we normalize the angle to [0, 2pi)
// and then flip the axis if it is in (pi, 2pi).
// we know phi_norm > 0
Scalar phi_norm_wrapped = fmod(phi_norm, 2 * M_PI);
if (phi_norm_wrapped > M_PI) {
// flip axis and invert angle
phi_norm_wrapped = 2 * M_PI - phi_norm_wrapped;
phi_hat =
Sophus::SO3<Scalar>::hat(-phi * (phi_norm_wrapped / phi_norm));
} else {
// already in [0, pi]
phi_hat = Sophus::SO3<Scalar>::hat(phi * (phi_norm_wrapped / phi_norm));
}
phi_norm = phi_norm_wrapped;
phi_norm2 = phi_norm * phi_norm;
} else {
// regular case: already in (0, pi]
phi_hat = Sophus::SO3<Scalar>::hat(phi);
}
Eigen::Matrix<Scalar, 3, 3> phi_hat2 = phi_hat * phi_hat;
J.setIdentity();
J += phi_hat / 2;
// Angle is now in range [0, pi]. We check if we are close to 0 (in case of
// a wrap-around) or pi and apply Taylor expansions to scalar multiplier of
// phi_hat2
if (phi_norm < Sophus::Constants<Scalar>::epsilon()) {
// 1st-order Taylor expansion around 0
J += phi_hat2 / 12;
} else if (M_PI - phi_norm < Sophus::Constants<Scalar>::epsilon()) {
// 0th-order Taylor expansion around pi
J += phi_hat2 / (M_PI * M_PI);
} else {
// regular case for range (0,pi)
J += phi_hat2 * (1 / phi_norm2 - (1 + std::cos(phi_norm)) /
(2 * phi_norm * std::sin(phi_norm)));
}
}
}
*/
/// @brief Left Jacobian for SO(3)
///
/// For \f$ \exp(x) \in SO(3) \f$ provides a Jacobian that approximates the sum
/// under expmap with a left multiplication of expmap for small \f$ \epsilon
/// \f$. Can be used to compute: \f$ \exp(\phi + \epsilon) \approx
/// \exp(J_{\phi} \epsilon) \exp(\phi) \f$
/// @param[in] phi (3x1 vector)
/// @param[out] J_phi (3x3 matrix)
template <typename Derived1, typename Derived2>
inline void leftJacobianSO3(const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
Scalar phi_norm2 = phi.squaredNorm();
Eigen::Matrix<Scalar, 3, 3> phi_hat = Sophus::SO3<Scalar>::hat(phi);
Eigen::Matrix<Scalar, 3, 3> phi_hat2 = phi_hat * phi_hat;
J.setIdentity();
if (phi_norm2 > Sophus::Constants<Scalar>::epsilon()) {
Scalar phi_norm = std::sqrt(phi_norm2);
Scalar phi_norm3 = phi_norm2 * phi_norm;
J += phi_hat * (1 - std::cos(phi_norm)) / phi_norm2;
J += phi_hat2 * (phi_norm - std::sin(phi_norm)) / phi_norm3;
} else {
// Taylor expansion around 0
J += phi_hat / 2;
J += phi_hat2 / 6;
}
}
/// @brief Left Inverse Jacobian for SO(3)
///
/// For \f$ \exp(x) \in SO(3) \f$ provides an inverse Jacobian that approximates
/// the logmap of the left multiplication of expmap of the arguments with a sum
/// for small \f$ \epsilon \f$. Can be used to compute: \f$ \log
/// (\exp(\epsilon) \exp(\phi)) \approx \phi + J_{\phi} \epsilon\f$
/// @param[in] phi (3x1 vector)
/// @param[out] J_phi (3x3 matrix)
template <typename Derived1, typename Derived2>
inline void leftJacobianInvSO3(const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
Scalar phi_norm2 = phi.squaredNorm();
Eigen::Matrix<Scalar, 3, 3> phi_hat = Sophus::SO3<Scalar>::hat(phi);
Eigen::Matrix<Scalar, 3, 3> phi_hat2 = phi_hat * phi_hat;
J.setIdentity();
J -= phi_hat / 2;
if (phi_norm2 > Sophus::Constants<Scalar>::epsilon()) {
Scalar phi_norm = std::sqrt(phi_norm2);
// We require that the angle is in range [0, pi]. We check if we are close
// to pi and apply a Taylor expansion to scalar multiplier of phi_hat2.
// Technically, log(exp(phi)exp(epsilon)) is not continuous / differentiable
// at phi=pi, but we still aim to return a reasonable value for all valid
// inputs.
BASALT_ASSERT(phi_norm <= M_PI + Sophus::Constants<Scalar>::epsilon());
if (phi_norm < M_PI - Sophus::Constants<Scalar>::epsilonSqrt()) {
// regular case for range (0,pi)
J += phi_hat2 * (1 / phi_norm2 - (1 + std::cos(phi_norm)) /
(2 * phi_norm * std::sin(phi_norm)));
} else {
// 0th-order Taylor expansion around pi
J += phi_hat2 / (M_PI * M_PI);
}
} else {
// Taylor expansion around 0
J += phi_hat2 / 12;
}
}
/// @brief Right Jacobian for decoupled SE(3)
///
/// For \f$ \exp(x) \in SE(3) \f$ provides a Jacobian that approximates the sum
/// under decoupled expmap with a right multiplication of decoupled expmap for
/// small \f$ \epsilon \f$. Can be used to compute: \f$ \exp(\phi + \epsilon)
/// \approx \exp(\phi) \exp(J_{\phi} \epsilon)\f$
/// @param[in] phi (6x1 vector)
/// @param[out] J_phi (6x6 matrix)
template <typename Derived1, typename Derived2>
inline void rightJacobianSE3Decoupled(
const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 6);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 6, 6);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
J.setZero();
Eigen::Matrix<Scalar, 3, 1> omega = phi.template tail<3>();
rightJacobianSO3(omega, J.template bottomRightCorner<3, 3>());
J.template topLeftCorner<3, 3>() =
Sophus::SO3<Scalar>::exp(omega).inverse().matrix();
}
/// @brief Right Inverse Jacobian for decoupled SE(3)
///
/// For \f$ \exp(x) \in SE(3) \f$ provides an inverse Jacobian that approximates
/// the decoupled logmap of the right multiplication of the decoupled expmap of
/// the arguments with a sum for small \f$ \epsilon \f$. Can be used to
/// compute: \f$ \log
/// (\exp(\phi) \exp(\epsilon)) \approx \phi + J_{\phi} \epsilon\f$
/// @param[in] phi (6x1 vector)
/// @param[out] J_phi (6x6 matrix)
template <typename Derived1, typename Derived2>
inline void rightJacobianInvSE3Decoupled(
const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 6);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 6, 6);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
J.setZero();
Eigen::Matrix<Scalar, 3, 1> omega = phi.template tail<3>();
rightJacobianInvSO3(omega, J.template bottomRightCorner<3, 3>());
J.template topLeftCorner<3, 3>() = Sophus::SO3<Scalar>::exp(omega).matrix();
}
/// @brief Right Jacobian for decoupled Sim(3)
///
/// For \f$ \exp(x) \in Sim(3) \f$ provides a Jacobian that approximates the sum
/// under decoupled expmap with a right multiplication of decoupled expmap for
/// small \f$ \epsilon \f$. Can be used to compute: \f$ \exp(\phi + \epsilon)
/// \approx \exp(\phi) \exp(J_{\phi} \epsilon)\f$
/// @param[in] phi (7x1 vector)
/// @param[out] J_phi (7x7 matrix)
template <typename Derived1, typename Derived2>
inline void rightJacobianSim3Decoupled(
const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 7);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 7, 7);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
J.setZero();
Eigen::Matrix<Scalar, 4, 1> omega = phi.template tail<4>();
rightJacobianSO3(omega.template head<3>(), J.template block<3, 3>(3, 3));
J.template topLeftCorner<3, 3>() =
Sophus::RxSO3<Scalar>::exp(omega).inverse().matrix();
J(6, 6) = 1;
}
/// @brief Right Inverse Jacobian for decoupled Sim(3)
///
/// For \f$ \exp(x) \in Sim(3) \f$ provides an inverse Jacobian that
/// approximates the decoupled logmap of the right multiplication of the
/// decoupled expmap of the arguments with a sum for small \f$ \epsilon \f$. Can
/// be used to compute: \f$ \log
/// (\exp(\phi) \exp(\epsilon)) \approx \phi + J_{\phi} \epsilon\f$
/// @param[in] phi (7x1 vector)
/// @param[out] J_phi (7x7 matrix)
template <typename Derived1, typename Derived2>
inline void rightJacobianInvSim3Decoupled(
const Eigen::MatrixBase<Derived1> &phi,
const Eigen::MatrixBase<Derived2> &J_phi) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1);
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 7);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 7, 7);
using Scalar = typename Derived1::Scalar;
Eigen::MatrixBase<Derived2> &J =
const_cast<Eigen::MatrixBase<Derived2> &>(J_phi);
J.setZero();
Eigen::Matrix<Scalar, 4, 1> omega = phi.template tail<4>();
rightJacobianInvSO3(omega.template head<3>(), J.template block<3, 3>(3, 3));
J.template topLeftCorner<3, 3>() = Sophus::RxSO3<Scalar>::exp(omega).matrix();
J(6, 6) = 1;
}
} // namespace Sophus