pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects 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

167
include/basalt/vi_estimator/ba_base.h Normal file
View File

@@ -0,0 +1,167 @@
/**
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 <basalt/vi_estimator/landmark_database.h>
namespace basalt {
template <class Scalar_>
class BundleAdjustmentBase {
public:
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 Vec6 = Eigen::Matrix<Scalar, 6, 1>;
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
using SE3 = Sophus::SE3<Scalar>;
void computeError(Scalar& error,
std::map<int, std::vector<std::pair<TimeCamId, Scalar>>>*
outliers = nullptr,
Scalar outlier_threshold = 0) const;
void filterOutliers(Scalar outlier_threshold, int min_num_obs);
void optimize_single_frame_pose(
PoseStateWithLin<Scalar>& state_t,
const std::vector<std::vector<int>>& connected_obs) const;
template <class Scalar2>
void get_current_points(
Eigen::aligned_vector<Eigen::Matrix<Scalar2, 3, 1>>& points,
std::vector<int>& ids) const;
void computeDelta(const AbsOrderMap& marg_order, VecX& delta) const;
void linearizeMargPrior(const MargLinData<Scalar>& mld,
const AbsOrderMap& aom, MatX& abs_H, VecX& abs_b,
Scalar& marg_prior_error) const;
void computeMargPriorError(const MargLinData<Scalar>& mld,
Scalar& marg_prior_error) const;
Scalar computeMargPriorModelCostChange(const MargLinData<Scalar>& mld,
const VecX& marg_scaling,
const VecX& marg_pose_inc) const;
// TODO: Old version for squared H and b. Remove when no longer needed.
Scalar computeModelCostChange(const MatX& H, const VecX& b,
const VecX& inc) const;
template <class Scalar2>
void computeProjections(
std::vector<Eigen::aligned_vector<Eigen::Matrix<Scalar2, 4, 1>>>& data,
FrameId last_state_t_ns) const;
/// Triangulates the point and returns homogenous representation. First 3
/// components - unit-length direction vector. Last component inverse
/// distance.
template <class Derived>
static Eigen::Matrix<typename Derived::Scalar, 4, 1> triangulate(
const Eigen::MatrixBase<Derived>& f0,
const Eigen::MatrixBase<Derived>& f1,
const Sophus::SE3<typename Derived::Scalar>& T_0_1) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
// suffix "2" to avoid name clash with class-wide typedefs
using Scalar_2 = typename Derived::Scalar;
using Vec4_2 = Eigen::Matrix<Scalar_2, 4, 1>;
Eigen::Matrix<Scalar_2, 3, 4> P1, P2;
P1.setIdentity();
P2 = T_0_1.inverse().matrix3x4();
Eigen::Matrix<Scalar_2, 4, 4> A(4, 4);
A.row(0) = f0[0] * P1.row(2) - f0[2] * P1.row(0);
A.row(1) = f0[1] * P1.row(2) - f0[2] * P1.row(1);
A.row(2) = f1[0] * P2.row(2) - f1[2] * P2.row(0);
A.row(3) = f1[1] * P2.row(2) - f1[2] * P2.row(1);
Eigen::JacobiSVD<Eigen::Matrix<Scalar_2, 4, 4>> mySVD(A,
Eigen::ComputeFullV);
Vec4_2 worldPoint = mySVD.matrixV().col(3);
worldPoint /= worldPoint.template head<3>().norm();
// Enforce same direction of bearing vector and initial point
if (f0.dot(worldPoint.template head<3>()) < 0) worldPoint *= -1;
return worldPoint;
}
inline void backup() {
for (auto& kv : frame_states) kv.second.backup();
for (auto& kv : frame_poses) kv.second.backup();
lmdb.backup();
}
inline void restore() {
for (auto& kv : frame_states) kv.second.restore();
for (auto& kv : frame_poses) kv.second.restore();
lmdb.restore();
}
// protected:
PoseStateWithLin<Scalar> getPoseStateWithLin(int64_t t_ns) const {
auto it = frame_poses.find(t_ns);
if (it != frame_poses.end()) return it->second;
auto it2 = frame_states.find(t_ns);
if (it2 == frame_states.end()) {
std::cerr << "Could not find pose " << t_ns << std::endl;
std::abort();
}
return PoseStateWithLin<Scalar>(it2->second);
}
Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>> frame_states;
Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>> frame_poses;
// Point management
LandmarkDatabase<Scalar> lmdb;
Scalar obs_std_dev;
Scalar huber_thresh;
basalt::Calibration<Scalar> calib;
};
} // namespace basalt

156
include/basalt/vi_estimator/landmark_database.h Normal file
View File

@@ -0,0 +1,156 @@
/**
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 <basalt/utils/imu_types.h>
#include <basalt/utils/eigen_utils.hpp>
namespace basalt {
template <class Scalar_>
struct KeypointObservation {
using Scalar = Scalar_;
int kpt_id;
Eigen::Matrix<Scalar, 2, 1> pos;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
// keypoint position defined relative to some frame
template <class Scalar_>
struct Keypoint {
using Scalar = Scalar_;
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
using ObsMap = Eigen::aligned_map<TimeCamId, Vec2>;
using MapIter = typename ObsMap::iterator;
// 3D position parameters
Vec2 direction;
Scalar inv_dist;
// Observations
TimeCamId host_kf_id;
ObsMap obs;
inline void backup() {
backup_direction = direction;
backup_inv_dist = inv_dist;
}
inline void restore() {
direction = backup_direction;
inv_dist = backup_inv_dist;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
Vec2 backup_direction;
Scalar backup_inv_dist;
};
template <class Scalar_>
class LandmarkDatabase {
public:
using Scalar = Scalar_;
// Non-const
void addLandmark(KeypointId lm_id, const Keypoint<Scalar>& pos);
void removeFrame(const FrameId& frame);
void removeKeyframes(const std::set<FrameId>& kfs_to_marg,
const std::set<FrameId>& poses_to_marg,
const std::set<FrameId>& states_to_marg_all);
void addObservation(const TimeCamId& tcid_target,
const KeypointObservation<Scalar>& o);
Keypoint<Scalar>& getLandmark(KeypointId lm_id);
// Const
const Keypoint<Scalar>& getLandmark(KeypointId lm_id) const;
std::vector<TimeCamId> getHostKfs() const;
std::vector<const Keypoint<Scalar>*> getLandmarksForHost(
const TimeCamId& tcid) const;
const std::unordered_map<TimeCamId,
std::map<TimeCamId, std::set<KeypointId>>>&
getObservations() const;
const Eigen::aligned_unordered_map<KeypointId, Keypoint<Scalar>>&
getLandmarks() const;
bool landmarkExists(int lm_id) const;
size_t numLandmarks() const;
int numObservations() const;
int numObservations(KeypointId lm_id) const;
void removeLandmark(KeypointId lm_id);
void removeObservations(KeypointId lm_id, const std::set<TimeCamId>& obs);
inline void backup() {
for (auto& kv : kpts) kv.second.backup();
}
inline void restore() {
for (auto& kv : kpts) kv.second.restore();
}
private:
using MapIter =
typename Eigen::aligned_unordered_map<KeypointId,
Keypoint<Scalar>>::iterator;
MapIter removeLandmarkHelper(MapIter it);
typename Keypoint<Scalar>::MapIter removeLandmarkObservationHelper(
MapIter it, typename Keypoint<Scalar>::MapIter it2);
Eigen::aligned_unordered_map<KeypointId, Keypoint<Scalar>> kpts;
std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
observations;
static constexpr int min_num_obs = 2;
};
} // namespace basalt

68
include/basalt/vi_estimator/marg_helper.h Normal file
View File

@@ -0,0 +1,68 @@
/**
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 <Eigen/Dense>
#include <set>
namespace basalt {
template <class Scalar_>
class MargHelper {
public:
using Scalar = Scalar_;
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
// Modifies abs_H and abs_b as a side effect.
static void marginalizeHelperSqToSq(MatX& abs_H, VecX& abs_b,
const std::set<int>& idx_to_keep,
const std::set<int>& idx_to_marg,
MatX& marg_H, VecX& marg_b);
// Modifies abs_H and abs_b as a side effect.
static void marginalizeHelperSqToSqrt(MatX& abs_H, VecX& abs_b,
const std::set<int>& idx_to_keep,
const std::set<int>& idx_to_marg,
MatX& marg_sqrt_H, VecX& marg_sqrt_b);
// Modifies Q2Jp and Q2r as a side effect.
static void marginalizeHelperSqrtToSqrt(MatX& Q2Jp, VecX& Q2r,
const std::set<int>& idx_to_keep,
const std::set<int>& idx_to_marg,
MatX& marg_sqrt_H, VecX& marg_sqrt_b);
};
} // namespace basalt

209
include/basalt/vi_estimator/nfr_mapper.h Normal file
View File

@@ -0,0 +1,209 @@
/**
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/utils/common_types.h>
#include <basalt/utils/nfr.h>
#include <basalt/vi_estimator/sc_ba_base.h>
#include <basalt/vi_estimator/vio_estimator.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/concurrent_vector.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>
namespace basalt {
template <size_t N>
class HashBow;
class NfrMapper : public ScBundleAdjustmentBase<double> {
public:
using Scalar = double;
using Ptr = std::shared_ptr<NfrMapper>;
template <class AccumT>
struct MapperLinearizeAbsReduce
: public ScBundleAdjustmentBase<Scalar>::LinearizeAbsReduce<AccumT> {
using RollPitchFactorConstIter =
Eigen::aligned_vector<RollPitchFactor>::const_iterator;
using RelPoseFactorConstIter =
Eigen::aligned_vector<RelPoseFactor>::const_iterator;
using RelLinDataConstIter =
Eigen::aligned_vector<RelLinData>::const_iterator;
MapperLinearizeAbsReduce(
AbsOrderMap& aom,
const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>*
frame_poses)
: ScBundleAdjustmentBase<Scalar>::LinearizeAbsReduce<AccumT>(aom),
frame_poses(frame_poses) {
this->accum.reset(aom.total_size);
roll_pitch_error = 0;
rel_error = 0;
}
MapperLinearizeAbsReduce(const MapperLinearizeAbsReduce& other, tbb::split)
: ScBundleAdjustmentBase<Scalar>::LinearizeAbsReduce<AccumT>(other.aom),
frame_poses(other.frame_poses) {
this->accum.reset(this->aom.total_size);
roll_pitch_error = 0;
rel_error = 0;
}
void join(MapperLinearizeAbsReduce& rhs) {
this->accum.join(rhs.accum);
roll_pitch_error += rhs.roll_pitch_error;
rel_error += rhs.rel_error;
}
void operator()(const tbb::blocked_range<RelLinDataConstIter>& range) {
for (const RelLinData& rld : range) {
Eigen::MatrixXd rel_H;
Eigen::VectorXd rel_b;
linearizeRel(rld, rel_H, rel_b);
linearizeAbs(rel_H, rel_b, rld, this->aom, this->accum);
}
}
void operator()(const tbb::blocked_range<RollPitchFactorConstIter>& range) {
for (const RollPitchFactor& rpf : range) {
const Sophus::SE3d& pose = frame_poses->at(rpf.t_ns).getPose();
int idx = this->aom.abs_order_map.at(rpf.t_ns).first;
Eigen::Matrix<double, 2, POSE_SIZE> J;
Sophus::Vector2d res = basalt::rollPitchError(pose, rpf.R_w_i_meas, &J);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx, idx, J.transpose() * rpf.cov_inv * J);
this->accum.template addB<POSE_SIZE>(idx,
J.transpose() * rpf.cov_inv * res);
roll_pitch_error += res.transpose() * rpf.cov_inv * res;
}
}
void operator()(const tbb::blocked_range<RelPoseFactorConstIter>& range) {
for (const RelPoseFactor& rpf : range) {
const Sophus::SE3d& pose_i = frame_poses->at(rpf.t_i_ns).getPose();
const Sophus::SE3d& pose_j = frame_poses->at(rpf.t_j_ns).getPose();
int idx_i = this->aom.abs_order_map.at(rpf.t_i_ns).first;
int idx_j = this->aom.abs_order_map.at(rpf.t_j_ns).first;
Sophus::Matrix6d Ji, Jj;
Sophus::Vector6d res =
basalt::relPoseError(rpf.T_i_j, pose_i, pose_j, &Ji, &Jj);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx_i, idx_i, Ji.transpose() * rpf.cov_inv * Ji);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx_i, idx_j, Ji.transpose() * rpf.cov_inv * Jj);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx_j, idx_i, Jj.transpose() * rpf.cov_inv * Ji);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx_j, idx_j, Jj.transpose() * rpf.cov_inv * Jj);
this->accum.template addB<POSE_SIZE>(
idx_i, Ji.transpose() * rpf.cov_inv * res);
this->accum.template addB<POSE_SIZE>(
idx_j, Jj.transpose() * rpf.cov_inv * res);
rel_error += res.transpose() * rpf.cov_inv * res;
}
}
double roll_pitch_error;
double rel_error;
const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>* frame_poses;
};
NfrMapper(const basalt::Calibration<double>& calib, const VioConfig& config);
void addMargData(basalt::MargData::Ptr& data);
void processMargData(basalt::MargData& m);
bool extractNonlinearFactors(basalt::MargData& m);
void optimize(int num_iterations = 10);
Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& getFramePoses();
void computeRelPose(double& rel_error);
void computeRollPitch(double& roll_pitch_error);
void detect_keypoints();
// Feature matching and inlier filtering for stereo pairs with known pose
void match_stereo();
void match_all();
void build_tracks();
void setup_opt();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::aligned_vector<RollPitchFactor> roll_pitch_factors;
Eigen::aligned_vector<RelPoseFactor> rel_pose_factors;
std::unordered_map<int64_t, OpticalFlowInput::Ptr> img_data;
Corners feature_corners;
Matches feature_matches;
FeatureTracks feature_tracks;
std::shared_ptr<HashBow<256>> hash_bow_database;
VioConfig config;
double lambda, min_lambda, max_lambda, lambda_vee;
};
} // namespace basalt

452
include/basalt/vi_estimator/sc_ba_base.h Normal file
View File

@@ -0,0 +1,452 @@
/**
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 <basalt/vi_estimator/ba_base.h>
#include <tbb/blocked_range.h>
namespace basalt {
template <class Scalar_>
class ScBundleAdjustmentBase : public BundleAdjustmentBase<Scalar_> {
public:
using Scalar = Scalar_;
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using Vec6 = Eigen::Matrix<Scalar, 6, 1>;
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using Mat3 = Eigen::Matrix<Scalar, 3, 3>;
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
using Mat63 = Eigen::Matrix<Scalar, 6, 3>;
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
using SO3 = Sophus::SO3<Scalar>;
using SE3 = Sophus::SE3<Scalar>;
struct RelLinDataBase {
std::vector<std::pair<TimeCamId, TimeCamId>> order;
Eigen::aligned_vector<Mat6> d_rel_d_h;
Eigen::aligned_vector<Mat6> d_rel_d_t;
};
struct FrameRelLinData {
Mat6 Hpp;
Vec6 bp;
std::vector<int> lm_id;
Eigen::aligned_vector<Mat63> Hpl;
FrameRelLinData() {
Hpp.setZero();
bp.setZero();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct RelLinData : public RelLinDataBase {
RelLinData(size_t num_keypoints, size_t num_rel_poses) {
Hll.reserve(num_keypoints);
Hllinv.reserve(num_keypoints);
bl.reserve(num_keypoints);
lm_to_obs.reserve(num_keypoints);
Hpppl.reserve(num_rel_poses);
order.reserve(num_rel_poses);
d_rel_d_h.reserve(num_rel_poses);
d_rel_d_t.reserve(num_rel_poses);
error = 0;
}
void invert_keypoint_hessians() {
for (const auto& [kpt_idx, hll] : Hll) {
Mat3 Hll_inv;
Hll_inv.setIdentity();
// Use ldlt b/c it has good speed (no numeric indefiniteness of this 3x3
// matrix expected), and compared ot a direct inverse (which may be even
// faster), it can handle degenerate cases where Hll is not invertible.
hll.ldlt().solveInPlace(Hll_inv);
Hllinv[kpt_idx] = Hll_inv;
}
}
using RelLinDataBase::d_rel_d_h;
using RelLinDataBase::d_rel_d_t;
using RelLinDataBase::order;
Eigen::aligned_unordered_map<int, Mat3> Hll;
Eigen::aligned_unordered_map<int, Mat3> Hllinv;
Eigen::aligned_unordered_map<int, Vec3> bl;
Eigen::aligned_unordered_map<int, std::vector<std::pair<size_t, size_t>>>
lm_to_obs;
Eigen::aligned_vector<FrameRelLinData> Hpppl;
Scalar error;
};
struct FrameAbsLinData {
Mat6 Hphph;
Vec6 bph;
Mat6 Hptpt;
Vec6 bpt;
Mat6 Hphpt;
std::vector<int> lm_id;
Eigen::aligned_vector<Mat63> Hphl;
Eigen::aligned_vector<Mat63> Hptl;
FrameAbsLinData() {
Hphph.setZero();
Hptpt.setZero();
Hphpt.setZero();
bph.setZero();
bpt.setZero();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct AbsLinData {
AbsLinData(size_t num_keypoints, size_t num_rel_poses) {
Hll.reserve(num_keypoints);
Hllinv.reserve(num_keypoints);
bl.reserve(num_keypoints);
lm_to_obs.reserve(num_keypoints);
Hpppl.reserve(num_rel_poses);
order.reserve(num_rel_poses);
error = 0;
}
void invert_keypoint_hessians() {
for (const auto& [kpt_idx, hll] : Hll) {
Mat3 Hll_inv;
Hll_inv.setIdentity();
// Use ldlt b/c it has good speed (no numeric indefiniteness of this 3x3
// matrix expected), and compared ot a direct inverse (which may be even
// faster), it can handle degenerate cases where Hll is not invertible.
hll.ldlt().solveInPlace(Hll_inv);
Hllinv[kpt_idx] = Hll_inv;
}
}
std::vector<std::pair<TimeCamId, TimeCamId>> order;
Eigen::aligned_unordered_map<int, Mat3> Hll;
Eigen::aligned_unordered_map<int, Mat3> Hllinv;
Eigen::aligned_unordered_map<int, Vec3> bl;
Eigen::aligned_unordered_map<int, std::vector<std::pair<size_t, size_t>>>
lm_to_obs;
Eigen::aligned_vector<FrameAbsLinData> Hpppl;
Scalar error;
};
using BundleAdjustmentBase<Scalar>::computeDelta;
void linearizeHelper(
Eigen::aligned_vector<RelLinData>& rld_vec,
const std::unordered_map<
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
Scalar& error) const {
linearizeHelperStatic(rld_vec, obs_to_lin, this, error);
}
void linearizeAbsHelper(
Eigen::aligned_vector<AbsLinData>& ald_vec,
const std::unordered_map<
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
Scalar& error) const {
linearizeHelperAbsStatic(ald_vec, obs_to_lin, this, error);
}
static void linearizeHelperStatic(
Eigen::aligned_vector<RelLinData>& rld_vec,
const std::unordered_map<
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error);
void linearizeHelperAbs(
Eigen::aligned_vector<AbsLinData>& ald_vec,
const std::unordered_map<
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
Scalar& error) const {
linearizeHelperAbsStatic(ald_vec, obs_to_lin, this, error);
}
static void linearizeHelperAbsStatic(
Eigen::aligned_vector<AbsLinData>& ald_vec,
const std::unordered_map<
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error);
static void linearizeRel(const RelLinData& rld, MatX& H, VecX& b);
static void updatePoints(const AbsOrderMap& aom, const RelLinData& rld,
const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
Scalar* l_diff = nullptr);
static void updatePointsAbs(const AbsOrderMap& aom, const AbsLinData& ald,
const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
Scalar* l_diff = nullptr);
static Eigen::VectorXd checkNullspace(
const MatX& H, const VecX& b, const AbsOrderMap& marg_order,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
frame_states,
const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses,
bool verbose = true);
static Eigen::VectorXd checkEigenvalues(const MatX& H, bool verbose = true);
static void computeImuError(
const AbsOrderMap& aom, Scalar& imu_error, Scalar& bg_error,
Scalar& ba_error,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<Scalar>>&
imu_meas,
const Vec3& gyro_bias_weight, const Vec3& accel_bias_weight,
const Vec3& g);
template <class AccumT>
static void linearizeAbs(const MatX& rel_H, const VecX& rel_b,
const RelLinDataBase& rld, const AbsOrderMap& aom,
AccumT& accum) {
// int asize = aom.total_size;
// BASALT_ASSERT(abs_H.cols() == asize);
// BASALT_ASSERT(abs_H.rows() == asize);
// BASALT_ASSERT(abs_b.rows() == asize);
for (size_t i = 0; i < rld.order.size(); i++) {
const TimeCamId& tcid_h = rld.order[i].first;
const TimeCamId& tcid_ti = rld.order[i].second;
int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
int abs_ti_idx = aom.abs_order_map.at(tcid_ti.frame_id).first;
accum.template addB<POSE_SIZE>(
abs_h_idx, rld.d_rel_d_h[i].transpose() *
rel_b.template segment<POSE_SIZE>(i * POSE_SIZE));
accum.template addB<POSE_SIZE>(
abs_ti_idx, rld.d_rel_d_t[i].transpose() *
rel_b.template segment<POSE_SIZE>(i * POSE_SIZE));
for (size_t j = 0; j < rld.order.size(); j++) {
BASALT_ASSERT(rld.order[i].first == rld.order[j].first);
const TimeCamId& tcid_tj = rld.order[j].second;
int abs_tj_idx = aom.abs_order_map.at(tcid_tj.frame_id).first;
if (tcid_h.frame_id == tcid_ti.frame_id ||
tcid_h.frame_id == tcid_tj.frame_id)
continue;
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_h_idx,
rld.d_rel_d_h[i].transpose() *
rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
POSE_SIZE * j) *
rld.d_rel_d_h[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_h_idx,
rld.d_rel_d_t[i].transpose() *
rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
POSE_SIZE * j) *
rld.d_rel_d_h[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_tj_idx,
rld.d_rel_d_h[i].transpose() *
rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
POSE_SIZE * j) *
rld.d_rel_d_t[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_tj_idx,
rld.d_rel_d_t[i].transpose() *
rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
POSE_SIZE * j) *
rld.d_rel_d_t[j]);
}
}
}
template <class AccumT>
static void linearizeAbs(const AbsLinData& ald, const AbsOrderMap& aom,
AccumT& accum) {
for (size_t i = 0; i < ald.order.size(); i++) {
const TimeCamId& tcid_h = ald.order[i].first;
const TimeCamId& tcid_ti = ald.order[i].second;
int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
int abs_ti_idx = aom.abs_order_map.at(tcid_ti.frame_id).first;
const FrameAbsLinData& fald = ald.Hpppl.at(i);
// Pose H and b part
accum.template addH<POSE_SIZE, POSE_SIZE>(abs_h_idx, abs_h_idx,
fald.Hphph);
accum.template addH<POSE_SIZE, POSE_SIZE>(abs_ti_idx, abs_ti_idx,
fald.Hptpt);
accum.template addH<POSE_SIZE, POSE_SIZE>(abs_h_idx, abs_ti_idx,
fald.Hphpt);
accum.template addH<POSE_SIZE, POSE_SIZE>(abs_ti_idx, abs_h_idx,
fald.Hphpt.transpose());
accum.template addB<POSE_SIZE>(abs_h_idx, fald.bph);
accum.template addB<POSE_SIZE>(abs_ti_idx, fald.bpt);
// Schur complement for landmark part
for (size_t j = 0; j < fald.lm_id.size(); j++) {
Eigen::Matrix<Scalar, POSE_SIZE, 3> H_phl_H_ll_inv, H_ptl_H_ll_inv;
int lm_id = fald.lm_id.at(j);
H_phl_H_ll_inv = fald.Hphl[j] * ald.Hllinv.at(lm_id);
H_ptl_H_ll_inv = fald.Hptl[j] * ald.Hllinv.at(lm_id);
accum.template addB<POSE_SIZE>(abs_h_idx,
-H_phl_H_ll_inv * ald.bl.at(lm_id));
accum.template addB<POSE_SIZE>(abs_ti_idx,
-H_ptl_H_ll_inv * ald.bl.at(lm_id));
const auto& other_obs = ald.lm_to_obs.at(lm_id);
for (size_t k = 0; k < other_obs.size(); k++) {
int other_frame_idx = other_obs[k].first;
int other_lm_idx = other_obs[k].second;
const FrameAbsLinData& fald_other = ald.Hpppl.at(other_frame_idx);
const TimeCamId& tcid_hk = ald.order.at(other_frame_idx).first;
const TimeCamId& tcid_tk = ald.order.at(other_frame_idx).second;
// Assume same host frame
BASALT_ASSERT(tcid_hk.frame_id == tcid_h.frame_id &&
tcid_hk.cam_id == tcid_h.cam_id);
int abs_tk_idx = aom.abs_order_map.at(tcid_tk.frame_id).first;
Eigen::Matrix<Scalar, 3, POSE_SIZE> H_l_ph_other =
fald_other.Hphl[other_lm_idx].transpose();
Eigen::Matrix<Scalar, 3, POSE_SIZE> H_l_pt_other =
fald_other.Hptl[other_lm_idx].transpose();
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_h_idx, -H_phl_H_ll_inv * H_l_ph_other);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_h_idx, -H_ptl_H_ll_inv * H_l_ph_other);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_tk_idx, -H_phl_H_ll_inv * H_l_pt_other);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_tk_idx, -H_ptl_H_ll_inv * H_l_pt_other);
}
}
}
}
template <class AccumT>
struct LinearizeAbsReduce {
static_assert(std::is_same_v<typename AccumT::Scalar, Scalar>);
using RelLinConstDataIter =
typename Eigen::aligned_vector<RelLinData>::const_iterator;
LinearizeAbsReduce(const AbsOrderMap& aom) : aom(aom) {
accum.reset(aom.total_size);
}
LinearizeAbsReduce(const LinearizeAbsReduce& other, tbb::split)
: aom(other.aom) {
accum.reset(aom.total_size);
}
void operator()(const tbb::blocked_range<RelLinConstDataIter>& range) {
for (const RelLinData& rld : range) {
MatX rel_H;
VecX rel_b;
linearizeRel(rld, rel_H, rel_b);
linearizeAbs(rel_H, rel_b, rld, aom, accum);
}
}
void join(LinearizeAbsReduce& rhs) { accum.join(rhs.accum); }
const AbsOrderMap& aom;
AccumT accum;
};
template <class AccumT>
struct LinearizeAbsReduce2 {
static_assert(std::is_same_v<typename AccumT::Scalar, Scalar>);
using AbsLinDataConstIter =
typename Eigen::aligned_vector<AbsLinData>::const_iterator;
LinearizeAbsReduce2(const AbsOrderMap& aom) : aom(aom) {
accum.reset(aom.total_size);
}
LinearizeAbsReduce2(const LinearizeAbsReduce2& other, tbb::split)
: aom(other.aom) {
accum.reset(aom.total_size);
}
void operator()(const tbb::blocked_range<AbsLinDataConstIter>& range) {
for (const AbsLinData& ald : range) {
linearizeAbs(ald, aom, accum);
}
}
void join(LinearizeAbsReduce2& rhs) { accum.join(rhs.accum); }
const AbsOrderMap& aom;
AccumT accum;
};
};
} // namespace basalt

124
include/basalt/vi_estimator/sqrt_ba_base.h Normal file
View File

@@ -0,0 +1,124 @@
/**
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 <basalt/vi_estimator/ba_base.h>
#include <basalt/vi_estimator/sc_ba_base.h>
namespace basalt {
template <class Scalar_>
class SqrtBundleAdjustmentBase : public BundleAdjustmentBase<Scalar_> {
public:
using Scalar = Scalar_;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using Mat3 = Eigen::Matrix<Scalar, 3, 3>;
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
using SO3 = Sophus::SO3<Scalar>;
using RelLinDataBase =
typename ScBundleAdjustmentBase<Scalar>::RelLinDataBase;
using FrameRelLinData =
typename ScBundleAdjustmentBase<Scalar>::FrameRelLinData;
using RelLinData = typename ScBundleAdjustmentBase<Scalar>::RelLinData;
using FrameAbsLinData =
typename ScBundleAdjustmentBase<Scalar>::FrameAbsLinData;
using AbsLinData = typename ScBundleAdjustmentBase<Scalar>::AbsLinData;
using BundleAdjustmentBase<Scalar>::computeDelta;
void linearizeHelper(
Eigen::aligned_vector<RelLinData>& rld_vec,
const std::unordered_map<
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
Scalar& error) const {
ScBundleAdjustmentBase<Scalar>::linearizeHelperStatic(rld_vec, obs_to_lin,
this, error);
}
void linearizeAbsHelper(
Eigen::aligned_vector<AbsLinData>& ald_vec,
const std::unordered_map<
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
Scalar& error) const {
ScBundleAdjustmentBase<Scalar>::linearizeHelperAbsStatic(
ald_vec, obs_to_lin, this, error);
}
static void linearizeRel(const RelLinData& rld, MatX& H, VecX& b) {
ScBundleAdjustmentBase<Scalar>::linearizeRel(rld, H, b);
}
static void updatePoints(const AbsOrderMap& aom, const RelLinData& rld,
const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
Scalar* l_diff = nullptr) {
ScBundleAdjustmentBase<Scalar>::updatePoints(aom, rld, inc, lmdb, l_diff);
}
static void updatePointsAbs(const AbsOrderMap& aom, const AbsLinData& ald,
const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
Scalar* l_diff = nullptr) {
ScBundleAdjustmentBase<Scalar>::updatePointsAbs(aom, ald, inc, lmdb,
l_diff);
}
static Eigen::VectorXd checkNullspace(
const MargLinData<Scalar_>& mld,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
frame_states,
const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses,
bool verbose = true);
static Eigen::VectorXd checkEigenvalues(const MargLinData<Scalar_>& mld,
bool verbose = true);
template <class AccumT>
static void linearizeAbs(const MatX& rel_H, const VecX& rel_b,
const RelLinDataBase& rld, const AbsOrderMap& aom,
AccumT& accum) {
return ScBundleAdjustmentBase<Scalar>::template linearizeAbs<AccumT>(
rel_H, rel_b, rld, aom, accum);
}
template <class AccumT>
static void linearizeAbs(const AbsLinData& ald, const AbsOrderMap& aom,
AccumT& accum) {
return ScBundleAdjustmentBase<Scalar>::template linearizeAbs<AccumT>(
ald, aom, accum);
}
};
} // namespace basalt

261
include/basalt/vi_estimator/sqrt_keypoint_vio.h Normal file
View File

@@ -0,0 +1,261 @@
/**
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 <thread>
#include <basalt/imu/preintegration.h>
#include <basalt/utils/time_utils.hpp>
#include <basalt/vi_estimator/sqrt_ba_base.h>
#include <basalt/vi_estimator/vio_estimator.h>
#include <basalt/imu/preintegration.h>
namespace basalt {
template <class Scalar_>
class SqrtKeypointVioEstimator : public VioEstimatorBase,
public SqrtBundleAdjustmentBase<Scalar_> {
public:
using Scalar = Scalar_;
typedef std::shared_ptr<SqrtKeypointVioEstimator> Ptr;
static const int N = 9;
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 VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using MatN3 = Eigen::Matrix<Scalar, N, 3>;
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
using SE3 = Sophus::SE3<Scalar>;
using typename SqrtBundleAdjustmentBase<Scalar>::RelLinData;
using typename SqrtBundleAdjustmentBase<Scalar>::AbsLinData;
using BundleAdjustmentBase<Scalar>::computeError;
using BundleAdjustmentBase<Scalar>::get_current_points;
using BundleAdjustmentBase<Scalar>::computeDelta;
using BundleAdjustmentBase<Scalar>::computeProjections;
using BundleAdjustmentBase<Scalar>::triangulate;
using BundleAdjustmentBase<Scalar>::backup;
using BundleAdjustmentBase<Scalar>::restore;
using BundleAdjustmentBase<Scalar>::getPoseStateWithLin;
using BundleAdjustmentBase<Scalar>::computeModelCostChange;
using SqrtBundleAdjustmentBase<Scalar>::linearizeHelper;
using SqrtBundleAdjustmentBase<Scalar>::linearizeAbsHelper;
using SqrtBundleAdjustmentBase<Scalar>::linearizeRel;
using SqrtBundleAdjustmentBase<Scalar>::linearizeAbs;
using SqrtBundleAdjustmentBase<Scalar>::updatePoints;
using SqrtBundleAdjustmentBase<Scalar>::updatePointsAbs;
using SqrtBundleAdjustmentBase<Scalar>::linearizeMargPrior;
using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorError;
using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorModelCostChange;
using SqrtBundleAdjustmentBase<Scalar>::checkNullspace;
using SqrtBundleAdjustmentBase<Scalar>::checkEigenvalues;
SqrtKeypointVioEstimator(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) override;
void initialize(const Eigen::Vector3d& bg,
const Eigen::Vector3d& ba) override;
virtual ~SqrtKeypointVioEstimator() { maybe_join(); }
inline void maybe_join() override {
if (processing_thread) {
processing_thread->join();
processing_thread.reset();
}
}
void addIMUToQueue(const ImuData<double>::Ptr& data) override;
void addVisionToQueue(const OpticalFlowResult::Ptr& data) override;
typename ImuData<Scalar>::Ptr popFromImuDataQueue();
bool measure(const OpticalFlowResult::Ptr& opt_flow_meas,
const typename IntegratedImuMeasurement<Scalar>::Ptr& meas);
// int64_t propagate();
// void addNewState(int64_t data_t_ns);
void optimize_and_marg(const std::map<int64_t, int>& num_points_connected,
const std::unordered_set<KeypointId>& lost_landmaks);
void marginalize(const std::map<int64_t, int>& num_points_connected,
const std::unordered_set<KeypointId>& lost_landmaks);
void optimize();
void debug_finalize() override;
void logMargNullspace();
Eigen::VectorXd checkMargNullspace() const;
Eigen::VectorXd checkMargEigenvalues() const;
int64_t get_t_ns() const {
return frame_states.at(last_state_t_ns).getState().t_ns;
}
const SE3& get_T_w_i() const {
return frame_states.at(last_state_t_ns).getState().T_w_i;
}
const Vec3& get_vel_w_i() const {
return frame_states.at(last_state_t_ns).getState().vel_w_i;
}
const PoseVelBiasState<Scalar>& get_state() const {
return frame_states.at(last_state_t_ns).getState();
}
PoseVelBiasState<Scalar> get_state(int64_t t_ns) const {
PoseVelBiasState<Scalar> 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;
}
void setMaxStates(size_t val) override { max_states = val; }
void setMaxKfs(size_t val) override { max_kfs = val; }
Eigen::aligned_vector<SE3> getFrameStates() const {
Eigen::aligned_vector<SE3> res;
for (const auto& kv : frame_states) {
res.push_back(kv.second.getState().T_w_i);
}
return res;
}
Eigen::aligned_vector<SE3> getFramePoses() const {
Eigen::aligned_vector<SE3> res;
for (const auto& kv : frame_poses) {
res.push_back(kv.second.getPose());
}
return res;
}
Eigen::aligned_map<int64_t, SE3> getAllPosesMap() const {
Eigen::aligned_map<int64_t, SE3> 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;
}
Sophus::SE3d getT_w_i_init() override {
return T_w_i_init.template cast<double>();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
using BundleAdjustmentBase<Scalar>::frame_poses;
using BundleAdjustmentBase<Scalar>::frame_states;
using BundleAdjustmentBase<Scalar>::lmdb;
using BundleAdjustmentBase<Scalar>::obs_std_dev;
using BundleAdjustmentBase<Scalar>::huber_thresh;
using BundleAdjustmentBase<Scalar>::calib;
private:
bool take_kf;
int frames_after_kf;
std::set<int64_t> kf_ids;
int64_t last_state_t_ns;
Eigen::aligned_map<int64_t, IntegratedImuMeasurement<Scalar>> imu_meas;
const Vec3 g;
// Input
Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
std::map<int64_t, int> num_points_kf;
// Marginalization
MargLinData<Scalar> marg_data;
// Used only for debug and log purporses.
MargLinData<Scalar> nullspace_marg_data;
Vec3 gyro_bias_sqrt_weight, accel_bias_sqrt_weight;
size_t max_states;
size_t max_kfs;
SE3 T_w_i_init;
bool initialized;
bool opt_started;
VioConfig config;
constexpr static Scalar vee_factor = Scalar(2.0);
constexpr static Scalar initial_vee = Scalar(2.0);
Scalar lambda, min_lambda, max_lambda, lambda_vee;
std::shared_ptr<std::thread> processing_thread;
// timing and stats
ExecutionStats stats_all_;
ExecutionStats stats_sums_;
};
} // namespace basalt

251
include/basalt/vi_estimator/sqrt_keypoint_vo.h Normal file
View File

@@ -0,0 +1,251 @@
/**
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 <thread>
#include <basalt/vi_estimator/sqrt_ba_base.h>
#include <basalt/vi_estimator/vio_estimator.h>
#include <basalt/utils/time_utils.hpp>
namespace basalt {
template <class Scalar_>
class SqrtKeypointVoEstimator : public VioEstimatorBase,
public SqrtBundleAdjustmentBase<Scalar_> {
public:
using Scalar = Scalar_;
typedef std::shared_ptr<SqrtKeypointVoEstimator> Ptr;
static const int N = 9;
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 VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using MatN3 = Eigen::Matrix<Scalar, N, 3>;
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
using SE3 = Sophus::SE3<Scalar>;
using typename SqrtBundleAdjustmentBase<Scalar>::RelLinData;
using typename SqrtBundleAdjustmentBase<Scalar>::AbsLinData;
using BundleAdjustmentBase<Scalar>::computeError;
using BundleAdjustmentBase<Scalar>::get_current_points;
using BundleAdjustmentBase<Scalar>::computeDelta;
using BundleAdjustmentBase<Scalar>::computeProjections;
using BundleAdjustmentBase<Scalar>::triangulate;
using BundleAdjustmentBase<Scalar>::backup;
using BundleAdjustmentBase<Scalar>::restore;
using BundleAdjustmentBase<Scalar>::getPoseStateWithLin;
using BundleAdjustmentBase<Scalar>::computeModelCostChange;
using SqrtBundleAdjustmentBase<Scalar>::linearizeHelper;
using SqrtBundleAdjustmentBase<Scalar>::linearizeAbsHelper;
using SqrtBundleAdjustmentBase<Scalar>::linearizeRel;
using SqrtBundleAdjustmentBase<Scalar>::linearizeAbs;
using SqrtBundleAdjustmentBase<Scalar>::updatePoints;
using SqrtBundleAdjustmentBase<Scalar>::updatePointsAbs;
using SqrtBundleAdjustmentBase<Scalar>::linearizeMargPrior;
using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorError;
using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorModelCostChange;
using SqrtBundleAdjustmentBase<Scalar>::checkNullspace;
using SqrtBundleAdjustmentBase<Scalar>::checkEigenvalues;
SqrtKeypointVoEstimator(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) override;
void initialize(const Eigen::Vector3d& bg,
const Eigen::Vector3d& ba) override;
virtual ~SqrtKeypointVoEstimator() { maybe_join(); }
inline void maybe_join() override {
if (processing_thread) {
processing_thread->join();
processing_thread.reset();
}
}
void addIMUToQueue(const ImuData<double>::Ptr& data) override;
void addVisionToQueue(const OpticalFlowResult::Ptr& data) override;
bool measure(const OpticalFlowResult::Ptr& opt_flow_meas, bool add_frame);
// int64_t propagate();
// void addNewState(int64_t data_t_ns);
void optimize_and_marg(const std::map<int64_t, int>& num_points_connected,
const std::unordered_set<KeypointId>& lost_landmaks);
void marginalize(const std::map<int64_t, int>& num_points_connected,
const std::unordered_set<KeypointId>& lost_landmaks);
void optimize();
void logMargNullspace();
Eigen::VectorXd checkMargNullspace() const;
Eigen::VectorXd checkMargEigenvalues() const;
int64_t get_t_ns() const {
return frame_states.at(last_state_t_ns).getState().t_ns;
}
const SE3& get_T_w_i() const {
return frame_states.at(last_state_t_ns).getState().T_w_i;
}
const Vec3& get_vel_w_i() const {
return frame_states.at(last_state_t_ns).getState().vel_w_i;
}
const PoseVelBiasState<Scalar>& get_state() const {
return frame_states.at(last_state_t_ns).getState();
}
PoseVelBiasState<Scalar> get_state(int64_t t_ns) const {
PoseVelBiasState<Scalar> 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;
}
void setMaxStates(size_t val) override { max_states = val; }
void setMaxKfs(size_t val) override { max_kfs = val; }
Eigen::aligned_vector<SE3> getFrameStates() const {
Eigen::aligned_vector<SE3> res;
for (const auto& kv : frame_states) {
res.push_back(kv.second.getState().T_w_i);
}
return res;
}
Eigen::aligned_vector<SE3> getFramePoses() const {
Eigen::aligned_vector<SE3> res;
for (const auto& kv : frame_poses) {
res.push_back(kv.second.getPose());
}
return res;
}
Eigen::aligned_map<int64_t, SE3> getAllPosesMap() const {
Eigen::aligned_map<int64_t, SE3> 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;
}
Sophus::SE3d getT_w_i_init() override {
return T_w_i_init.template cast<double>();
}
void debug_finalize() override;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
using BundleAdjustmentBase<Scalar>::frame_poses;
using BundleAdjustmentBase<Scalar>::frame_states;
using BundleAdjustmentBase<Scalar>::lmdb;
using BundleAdjustmentBase<Scalar>::obs_std_dev;
using BundleAdjustmentBase<Scalar>::huber_thresh;
using BundleAdjustmentBase<Scalar>::calib;
private:
bool take_kf; // true if next frame should become kf
int frames_after_kf; // number of frames since last kf
std::set<int64_t> kf_ids; // sliding window frame ids
// timestamp of latest state in the sliding window
// TODO: check and document when this is equal to kf_ids.rbegin() and when
// not. It may be only relevant for VIO, not VO.
int64_t last_state_t_ns = -1;
// Input
Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
std::map<int64_t, int> num_points_kf;
// Marginalization
MargLinData<Scalar> marg_data;
// Used only for debug and log purporses.
MargLinData<Scalar> nullspace_marg_data;
size_t max_states;
size_t max_kfs;
SE3 T_w_i_init;
bool initialized;
VioConfig config;
constexpr static Scalar vee_factor = Scalar(2.0);
constexpr static Scalar initial_vee = Scalar(2.0);
Scalar lambda, min_lambda, max_lambda, lambda_vee;
std::shared_ptr<std::thread> processing_thread;
// timing and stats
ExecutionStats stats_all_;
ExecutionStats stats_sums_;
};
} // namespace basalt

136
include/basalt/vi_estimator/vio_estimator.h Normal file
View File

@@ -0,0 +1,136 @@
/**
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 <atomic>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/utils/imu_types.h>
namespace basalt {
struct VioVisualizationData {
typedef std::shared_ptr<VioVisualizationData> Ptr;
int64_t t_ns;
Eigen::aligned_vector<Sophus::SE3d> states;
Eigen::aligned_vector<Sophus::SE3d> frames;
Eigen::aligned_vector<Eigen::Vector3d> points;
std::vector<int> point_ids;
OpticalFlowResult::Ptr opt_flow_res;
std::vector<Eigen::aligned_vector<Eigen::Vector4d>> projections;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class VioEstimatorBase {
public:
typedef std::shared_ptr<VioEstimatorBase> Ptr;
VioEstimatorBase()
: out_state_queue(nullptr),
out_marg_queue(nullptr),
out_vis_queue(nullptr) {
vision_data_queue.set_capacity(10);
imu_data_queue.set_capacity(300);
last_processed_t_ns = 0;
finished = false;
}
std::atomic<int64_t> last_processed_t_ns;
std::atomic<bool> finished;
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr> vision_data_queue;
tbb::concurrent_bounded_queue<ImuData<double>::Ptr> imu_data_queue;
tbb::concurrent_bounded_queue<PoseVelBiasState<double>::Ptr>*
out_state_queue = nullptr;
tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue = nullptr;
tbb::concurrent_bounded_queue<VioVisualizationData::Ptr>* out_vis_queue =
nullptr;
virtual 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) = 0;
virtual void initialize(const Eigen::Vector3d& bg,
const Eigen::Vector3d& ba) = 0;
virtual void maybe_join() = 0;
virtual inline void drain_input_queues() {
// Input threads should abort when vio is finished, but might be stuck in
// full push to full queue. So this can help to drain queues after joining
// the processing thread.
while (!imu_data_queue.empty()) {
ImuData<double>::Ptr d;
imu_data_queue.pop(d);
}
while (!vision_data_queue.empty()) {
OpticalFlowResult::Ptr d;
vision_data_queue.pop(d);
}
}
virtual inline void debug_finalize() {}
virtual Sophus::SE3d getT_w_i_init() = 0;
// Legacy functions. Should not be used in the new code.
virtual void setMaxStates(size_t val) = 0;
virtual void setMaxKfs(size_t val) = 0;
virtual void addIMUToQueue(const ImuData<double>::Ptr& data) = 0;
virtual void addVisionToQueue(const OpticalFlowResult::Ptr& data) = 0;
};
class VioEstimatorFactory {
public:
static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config,
const Calibration<double>& cam,
const Eigen::Vector3d& g,
bool use_imu, bool use_double);
};
double alignSVD(const std::vector<int64_t>& filter_t_ns,
const Eigen::aligned_vector<Eigen::Vector3d>& filter_t_w_i,
const std::vector<int64_t>& gt_t_ns,
Eigen::aligned_vector<Eigen::Vector3d>& gt_t_w_i);
} // namespace basalt