196 lines
6.4 KiB
C++
196 lines
6.4 KiB
C++
/**
|
|
BSD 3-Clause License
|
|
|
|
This file is part of the Basalt project.
|
|
https://gitlab.com/VladyslavUsenko/basalt.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.
|
|
*/
|
|
#ifndef BASALT_LINEARIZE_H
|
|
#define BASALT_LINEARIZE_H
|
|
|
|
#include <basalt/io/dataset_io.h>
|
|
#include <basalt/spline/se3_spline.h>
|
|
#include <basalt/calibration/calibration.hpp>
|
|
#include <basalt/camera/stereographic_param.hpp>
|
|
|
|
namespace basalt {
|
|
|
|
template <typename Scalar>
|
|
struct LinearizeBase {
|
|
static const int POSE_SIZE = 6;
|
|
|
|
static const int POS_SIZE = 3;
|
|
static const int POS_OFFSET = 0;
|
|
static const int ROT_SIZE = 3;
|
|
static const int ROT_OFFSET = 3;
|
|
static const int ACCEL_BIAS_SIZE = 9;
|
|
static const int GYRO_BIAS_SIZE = 12;
|
|
static const int G_SIZE = 3;
|
|
|
|
static const int TIME_SIZE = 1;
|
|
|
|
static const int ACCEL_BIAS_OFFSET = 0;
|
|
static const int GYRO_BIAS_OFFSET = ACCEL_BIAS_SIZE;
|
|
static const int G_OFFSET = GYRO_BIAS_OFFSET + GYRO_BIAS_SIZE;
|
|
|
|
typedef Sophus::SE3<Scalar> SE3;
|
|
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
|
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
|
|
|
|
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
|
|
typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
|
|
|
|
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
|
|
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
|
|
|
|
typedef typename Eigen::aligned_vector<PoseData>::const_iterator PoseDataIter;
|
|
typedef typename Eigen::aligned_vector<GyroData>::const_iterator GyroDataIter;
|
|
typedef
|
|
typename Eigen::aligned_vector<AccelData>::const_iterator AccelDataIter;
|
|
typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
|
|
AprilgridCornersDataIter;
|
|
|
|
template <int INTRINSICS_SIZE>
|
|
struct PoseCalibH {
|
|
Sophus::Matrix6d H_pose_accum;
|
|
Sophus::Vector6d b_pose_accum;
|
|
Eigen::Matrix<double, INTRINSICS_SIZE, 6> H_intr_pose_accum;
|
|
Eigen::Matrix<double, INTRINSICS_SIZE, INTRINSICS_SIZE> H_intr_accum;
|
|
Eigen::Matrix<double, INTRINSICS_SIZE, 1> b_intr_accum;
|
|
|
|
PoseCalibH() {
|
|
H_pose_accum.setZero();
|
|
b_pose_accum.setZero();
|
|
H_intr_pose_accum.setZero();
|
|
H_intr_accum.setZero();
|
|
b_intr_accum.setZero();
|
|
}
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
};
|
|
|
|
struct CalibCommonData {
|
|
const Calibration<Scalar>* calibration = nullptr;
|
|
const MocapCalibration<Scalar>* mocap_calibration = nullptr;
|
|
const Eigen::aligned_vector<Eigen::Vector4d>* aprilgrid_corner_pos_3d =
|
|
nullptr;
|
|
|
|
// Calib data
|
|
const std::vector<size_t>* offset_T_i_c = nullptr;
|
|
const std::vector<size_t>* offset_intrinsics = nullptr;
|
|
|
|
// Cam data
|
|
size_t mocap_block_offset;
|
|
size_t bias_block_offset;
|
|
const std::unordered_map<int64_t, size_t>* offset_poses = nullptr;
|
|
|
|
// Cam-IMU data
|
|
const Vector3* g = nullptr;
|
|
|
|
bool opt_intrinsics;
|
|
|
|
// Cam-IMU options
|
|
bool opt_cam_time_offset;
|
|
bool opt_g;
|
|
bool opt_imu_scale;
|
|
|
|
Scalar pose_var_inv;
|
|
Vector3 gyro_var_inv;
|
|
Vector3 accel_var_inv;
|
|
Scalar mocap_var_inv;
|
|
|
|
Scalar huber_thresh;
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
};
|
|
|
|
template <class CamT>
|
|
inline void linearize_point(const Eigen::Vector2d& corner_pos, int corner_id,
|
|
const Eigen::Matrix4d& T_c_w, const CamT& cam,
|
|
PoseCalibH<CamT::N>* cph, double& error,
|
|
int& num_points, double& reproj_err) const {
|
|
Eigen::Matrix<double, 2, 4> d_r_d_p;
|
|
Eigen::Matrix<double, 2, CamT::N> d_r_d_param;
|
|
|
|
BASALT_ASSERT_STREAM(
|
|
corner_id < int(common_data.aprilgrid_corner_pos_3d->size()),
|
|
"corner_id " << corner_id);
|
|
|
|
Eigen::Vector4d point3d =
|
|
T_c_w * common_data.aprilgrid_corner_pos_3d->at(corner_id);
|
|
|
|
Eigen::Vector2d proj;
|
|
bool valid;
|
|
if (cph) {
|
|
valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param);
|
|
} else {
|
|
valid = cam.project(point3d, proj);
|
|
}
|
|
if (!valid || !proj.array().isFinite().all()) return;
|
|
|
|
Eigen::Vector2d residual = proj - corner_pos;
|
|
|
|
double e = residual.norm();
|
|
double huber_weight =
|
|
e < common_data.huber_thresh ? 1.0 : common_data.huber_thresh / e;
|
|
|
|
if (cph) {
|
|
Eigen::Matrix<double, 4, 6> d_point_d_xi;
|
|
|
|
d_point_d_xi.topLeftCorner<3, 3>().setIdentity();
|
|
d_point_d_xi.topRightCorner<3, 3>() =
|
|
-Sophus::SO3d::hat(point3d.head<3>());
|
|
d_point_d_xi.row(3).setZero();
|
|
|
|
Eigen::Matrix<double, 2, 6> d_r_d_xi = d_r_d_p * d_point_d_xi;
|
|
|
|
cph->H_pose_accum += huber_weight * d_r_d_xi.transpose() * d_r_d_xi;
|
|
cph->b_pose_accum += huber_weight * d_r_d_xi.transpose() * residual;
|
|
|
|
cph->H_intr_pose_accum +=
|
|
huber_weight * d_r_d_param.transpose() * d_r_d_xi;
|
|
cph->H_intr_accum += huber_weight * d_r_d_param.transpose() * d_r_d_param;
|
|
cph->b_intr_accum += huber_weight * d_r_d_param.transpose() * residual;
|
|
}
|
|
|
|
error += huber_weight * e * e * (2 - huber_weight);
|
|
reproj_err += e;
|
|
num_points++;
|
|
}
|
|
|
|
CalibCommonData common_data;
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
};
|
|
|
|
} // namespace basalt
|
|
|
|
#endif
|