236 lines
7.2 KiB
C++
236 lines
7.2 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.
|
|
*/
|
|
#pragma once
|
|
|
|
#include <memory>
|
|
#include <thread>
|
|
|
|
#include <Eigen/Dense>
|
|
#include <sophus/se3.hpp>
|
|
|
|
#include <basalt/imu/preintegration.h>
|
|
#include <basalt/io/dataset_io.h>
|
|
#include <basalt/utils/assert.h>
|
|
#include <basalt/utils/test_utils.h>
|
|
#include <basalt/camera/generic_camera.hpp>
|
|
#include <basalt/camera/stereographic_param.hpp>
|
|
#include <basalt/utils/sophus_utils.hpp>
|
|
|
|
#include <basalt/vi_estimator/ba_base.h>
|
|
#include <basalt/vi_estimator/vio_estimator.h>
|
|
|
|
namespace basalt {
|
|
|
|
class KeypointVioEstimator : public VioEstimatorBase,
|
|
public BundleAdjustmentBase {
|
|
public:
|
|
typedef std::shared_ptr<KeypointVioEstimator> Ptr;
|
|
|
|
static const int N = 9;
|
|
typedef Eigen::Matrix<double, N, 1> VecN;
|
|
typedef Eigen::Matrix<double, N, N> MatNN;
|
|
typedef Eigen::Matrix<double, N, 3> MatN3;
|
|
|
|
KeypointVioEstimator(double int_std_dev, const Eigen::Vector3d& g,
|
|
const basalt::Calibration<double>& calib,
|
|
const VioConfig& config);
|
|
|
|
void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
|
|
const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bg,
|
|
const Eigen::Vector3d& ba);
|
|
|
|
void initialize(const Eigen::Vector3d& bg, const Eigen::Vector3d& ba);
|
|
|
|
virtual ~KeypointVioEstimator() { processing_thread->join(); }
|
|
|
|
void addIMUToQueue(const ImuData::Ptr& data);
|
|
void addVisionToQueue(const OpticalFlowResult::Ptr& data);
|
|
|
|
bool measure(const OpticalFlowResult::Ptr& data,
|
|
const IntegratedImuMeasurement::Ptr& meas);
|
|
|
|
static void linearizeAbsIMU(
|
|
const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
|
|
double& imu_error, double& bg_error, double& ba_error,
|
|
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& states,
|
|
const Eigen::map<int64_t, IntegratedImuMeasurement>& imu_meas,
|
|
const Eigen::Vector3d& gyro_bias_weight,
|
|
const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g);
|
|
|
|
static void computeImuError(
|
|
const AbsOrderMap& aom, double& imu_error, double& bg_error,
|
|
double& ba_error,
|
|
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& states,
|
|
const Eigen::map<int64_t, IntegratedImuMeasurement>& imu_meas,
|
|
const Eigen::Vector3d& gyro_bias_weight,
|
|
const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g);
|
|
|
|
void linearizeMargPrior(const AbsOrderMap& aom, Eigen::MatrixXd& abs_H,
|
|
Eigen::VectorXd& abs_b,
|
|
double& marg_prior_error) const;
|
|
|
|
void computeMargPriorError(double& marg_prior_error) const;
|
|
|
|
void computeDelta(const AbsOrderMap& marg_order,
|
|
Eigen::VectorXd& delta) const;
|
|
|
|
// int64_t propagate();
|
|
// void addNewState(int64_t data_t_ns);
|
|
|
|
void marginalize(const std::map<int64_t, int>& num_points_connected);
|
|
|
|
void optimize();
|
|
|
|
void checkMargNullspace() const;
|
|
static Eigen::VectorXd checkNullspace(
|
|
const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b,
|
|
const AbsOrderMap& marg_order,
|
|
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& frame_states,
|
|
const Eigen::map<int64_t, PoseStateWithLin>& frame_poses);
|
|
|
|
int64_t get_t_ns() const {
|
|
return frame_states.at(last_state_t_ns).getState().t_ns;
|
|
}
|
|
const Sophus::SE3d& get_T_w_i() const {
|
|
return frame_states.at(last_state_t_ns).getState().T_w_i;
|
|
}
|
|
const Eigen::Vector3d& get_vel_w_i() const {
|
|
return frame_states.at(last_state_t_ns).getState().vel_w_i;
|
|
}
|
|
|
|
const PoseVelBiasState& get_state() const {
|
|
return frame_states.at(last_state_t_ns).getState();
|
|
}
|
|
PoseVelBiasState get_state(int64_t t_ns) const {
|
|
PoseVelBiasState state;
|
|
|
|
auto it = frame_states.find(t_ns);
|
|
|
|
if (it != frame_states.end()) {
|
|
return it->second.getState();
|
|
}
|
|
|
|
auto it2 = frame_poses.find(t_ns);
|
|
if (it2 != frame_poses.end()) {
|
|
state.T_w_i = it2->second.getPose();
|
|
}
|
|
|
|
return state;
|
|
}
|
|
// const MatNN get_cov() const { return cov.bottomRightCorner<N, N>(); }
|
|
|
|
void computeProjections(
|
|
std::vector<Eigen::vector<Eigen::Vector4d>>& res) const;
|
|
|
|
inline void setMaxStates(size_t val) { max_states = val; }
|
|
inline void setMaxKfs(size_t val) { max_kfs = val; }
|
|
|
|
Eigen::vector<Sophus::SE3d> getFrameStates() const {
|
|
Eigen::vector<Sophus::SE3d> res;
|
|
|
|
for (const auto& kv : frame_states) {
|
|
res.push_back(kv.second.getState().T_w_i);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
Eigen::vector<Sophus::SE3d> getFramePoses() const {
|
|
Eigen::vector<Sophus::SE3d> res;
|
|
|
|
for (const auto& kv : frame_poses) {
|
|
res.push_back(kv.second.getPose());
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
Eigen::map<int64_t, Sophus::SE3d> getAllPosesMap() const {
|
|
Eigen::map<int64_t, Sophus::SE3d> res;
|
|
|
|
for (const auto& kv : frame_poses) {
|
|
res[kv.first] = kv.second.getPose();
|
|
}
|
|
|
|
for (const auto& kv : frame_states) {
|
|
res[kv.first] = kv.second.getState().T_w_i;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
const Sophus::SE3d& getT_w_i_init() { return T_w_i_init; }
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
private:
|
|
bool take_kf;
|
|
int frames_after_kf;
|
|
std::set<int64_t> kf_ids;
|
|
|
|
int64_t last_state_t_ns;
|
|
Eigen::map<int64_t, IntegratedImuMeasurement> imu_meas;
|
|
|
|
const Eigen::Vector3d g;
|
|
|
|
// Input
|
|
|
|
Eigen::map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
|
|
|
|
std::map<int64_t, int> num_points_kf;
|
|
|
|
// Marginalization
|
|
AbsOrderMap marg_order;
|
|
Eigen::MatrixXd marg_H;
|
|
Eigen::VectorXd marg_b;
|
|
|
|
Eigen::Vector3d gyro_bias_weight, accel_bias_weight;
|
|
|
|
size_t max_states;
|
|
size_t max_kfs;
|
|
|
|
Sophus::SE3d T_w_i_init;
|
|
|
|
bool initialized;
|
|
bool opt_started;
|
|
|
|
VioConfig config;
|
|
|
|
int64_t msckf_kf_id;
|
|
|
|
std::shared_ptr<std::thread> processing_thread;
|
|
};
|
|
} // namespace basalt
|