v01
This commit is contained in:
Ivan
57
include/basalt/calibration/aprilgrid.h
Normal file
57
include/basalt/calibration/aprilgrid.h
Normal file
@@ -0,0 +1,57 @@
|
||||
/**
|
||||
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/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
struct AprilGrid {
|
||||
AprilGrid(const std::string &config_path);
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
|
||||
Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d;
|
||||
|
||||
inline int getTagCols() const { return tagCols; }
|
||||
inline int getTagRows() const { return tagRows; }
|
||||
|
||||
private:
|
||||
int tagCols; // number of apriltags
|
||||
int tagRows; // number of apriltags
|
||||
double tagSize; // size of apriltag, edge to edge [m]
|
||||
double tagSpacing; // ratio of space between tags to tagSize
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
131
include/basalt/calibration/calibration_helper.h
Normal file
131
include/basalt/calibration/calibration_helper.h
Normal file
@@ -0,0 +1,131 @@
|
||||
/**
|
||||
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/calibration/aprilgrid.h>
|
||||
#include <basalt/io/dataset_io.h>
|
||||
#include <basalt/utils/common_types.h>
|
||||
#include <basalt/calibration/calibration.hpp>
|
||||
|
||||
#include <tbb/concurrent_unordered_map.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
struct CalibCornerData {
|
||||
Eigen::aligned_vector<Eigen::Vector2d> corners;
|
||||
std::vector<int> corner_ids;
|
||||
std::vector<double> radii; //!< threshold used for maximum displacement
|
||||
//! during sub-pix refinement; Search region is
|
||||
//! slightly larger.
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct ProjectedCornerData {
|
||||
Eigen::aligned_vector<Eigen::Vector2d> corners_proj;
|
||||
std::vector<bool> corners_proj_success;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct CalibInitPoseData {
|
||||
Sophus::SE3d T_a_c;
|
||||
size_t num_inliers;
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector2d> reprojected_corners;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
using CalibCornerMap = tbb::concurrent_unordered_map<TimeCamId, CalibCornerData,
|
||||
std::hash<TimeCamId>>;
|
||||
|
||||
using CalibInitPoseMap =
|
||||
tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData,
|
||||
std::hash<TimeCamId>>;
|
||||
|
||||
class CalibHelper {
|
||||
public:
|
||||
static void detectCorners(const VioDatasetPtr& vio_data,
|
||||
CalibCornerMap& calib_corners,
|
||||
CalibCornerMap& calib_corners_rejected);
|
||||
|
||||
static void initCamPoses(
|
||||
const Calibration<double>::Ptr& calib,
|
||||
const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
|
||||
CalibCornerMap& calib_corners, CalibInitPoseMap& calib_init_poses);
|
||||
|
||||
static bool initializeIntrinsics(
|
||||
const Eigen::aligned_vector<Eigen::Vector2d>& corners,
|
||||
const std::vector<int>& corner_ids, const AprilGrid& aprilgrid, int cols,
|
||||
int rows, Eigen::Vector4d& init_intr);
|
||||
|
||||
static bool initializeIntrinsicsPinhole(
|
||||
const std::vector<CalibCornerData*> pinhole_corners,
|
||||
const AprilGrid& aprilgrid, int cols, int rows,
|
||||
Eigen::Vector4d& init_intr);
|
||||
|
||||
private:
|
||||
inline static double square(double x) { return x * x; }
|
||||
|
||||
inline static double hypot(double a, double b) {
|
||||
return sqrt(square(a) + square(b));
|
||||
}
|
||||
|
||||
static void computeInitialPose(
|
||||
const Calibration<double>::Ptr& calib, size_t cam_id,
|
||||
const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
|
||||
const basalt::CalibCornerData& cd, basalt::CalibInitPoseData& cp);
|
||||
|
||||
static size_t computeReprojectionError(
|
||||
const UnifiedCamera<double>& cam_calib,
|
||||
const Eigen::aligned_vector<Eigen::Vector2d>& corners,
|
||||
const std::vector<int>& corner_ids,
|
||||
const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
|
||||
const Sophus::SE3d& T_target_camera, double& error);
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
namespace cereal {
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::CalibCornerData& c) {
|
||||
ar(c.corners, c.corner_ids, c.radii);
|
||||
}
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::CalibInitPoseData& c) {
|
||||
ar(c.T_a_c, c.num_inliers, c.reprojected_corners);
|
||||
}
|
||||
} // namespace cereal
|
||||
176
include/basalt/calibration/cam_calib.h
Normal file
176
include/basalt/calibration/cam_calib.h
Normal file
@@ -0,0 +1,176 @@
|
||||
/**
|
||||
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 <pangolin/display/image_view.h>
|
||||
#include <pangolin/gl/gldraw.h>
|
||||
#include <pangolin/image/image.h>
|
||||
#include <pangolin/image/image_io.h>
|
||||
#include <pangolin/image/typed_image.h>
|
||||
#include <pangolin/pangolin.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <thread>
|
||||
|
||||
#include <basalt/calibration/aprilgrid.h>
|
||||
#include <basalt/calibration/calibration_helper.h>
|
||||
#include <basalt/image/image.h>
|
||||
#include <basalt/utils/test_utils.h>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class PosesOptimization;
|
||||
|
||||
class CamCalib {
|
||||
public:
|
||||
CamCalib(const std::string &dataset_path, const std::string &dataset_type,
|
||||
const std::string &aprilgrid_path, const std::string &cache_path,
|
||||
const std::string &cache_dataset_name, int skip_images,
|
||||
const std::vector<std::string> &cam_types, bool show_gui = true);
|
||||
|
||||
~CamCalib();
|
||||
|
||||
void initGui();
|
||||
|
||||
void computeVign();
|
||||
|
||||
void setNumCameras(size_t n);
|
||||
|
||||
void renderingLoop();
|
||||
|
||||
void computeProjections();
|
||||
|
||||
void detectCorners();
|
||||
|
||||
void initCamIntrinsics();
|
||||
|
||||
void initCamPoses();
|
||||
|
||||
void initCamExtrinsics();
|
||||
|
||||
void initOptimization();
|
||||
|
||||
void loadDataset();
|
||||
|
||||
void optimize();
|
||||
|
||||
bool optimizeWithParam(bool print_info,
|
||||
std::map<std::string, double> *stats = nullptr);
|
||||
|
||||
void saveCalib();
|
||||
|
||||
void drawImageOverlay(pangolin::View &v, size_t cam_id);
|
||||
|
||||
bool hasCorners() const;
|
||||
|
||||
void setOptIntrinsics(bool opt) { opt_intr = opt; }
|
||||
|
||||
private:
|
||||
static constexpr int UI_WIDTH = 300;
|
||||
|
||||
static constexpr size_t RANSAC_THRESHOLD = 10;
|
||||
|
||||
// typedef Calibration::Ptr CalibrationPtr;
|
||||
|
||||
VioDatasetPtr vio_dataset;
|
||||
// CalibrationPtr calib;
|
||||
|
||||
CalibCornerMap calib_corners;
|
||||
CalibCornerMap calib_corners_rejected;
|
||||
CalibInitPoseMap calib_init_poses;
|
||||
|
||||
std::shared_ptr<std::thread> processing_thread;
|
||||
|
||||
std::shared_ptr<PosesOptimization> calib_opt;
|
||||
|
||||
std::map<TimeCamId, ProjectedCornerData> reprojected_corners;
|
||||
std::map<TimeCamId, ProjectedCornerData> reprojected_vignette;
|
||||
std::map<TimeCamId, std::vector<double>> reprojected_vignette_error;
|
||||
|
||||
std::string dataset_path;
|
||||
std::string dataset_type;
|
||||
|
||||
AprilGrid april_grid;
|
||||
|
||||
std::string cache_path;
|
||||
std::string cache_dataset_name;
|
||||
|
||||
int skip_images;
|
||||
|
||||
std::vector<std::string> cam_types;
|
||||
|
||||
bool show_gui;
|
||||
|
||||
const size_t MIN_CORNERS = 15;
|
||||
|
||||
//////////////////////
|
||||
|
||||
pangolin::Var<int> show_frame;
|
||||
|
||||
pangolin::Var<bool> show_corners;
|
||||
pangolin::Var<bool> show_corners_rejected;
|
||||
pangolin::Var<bool> show_init_reproj;
|
||||
pangolin::Var<bool> show_opt;
|
||||
pangolin::Var<bool> show_vign;
|
||||
pangolin::Var<bool> show_ids;
|
||||
|
||||
pangolin::Var<double> huber_thresh;
|
||||
|
||||
pangolin::Var<bool> opt_intr;
|
||||
|
||||
pangolin::Var<bool> opt_until_convg;
|
||||
pangolin::Var<double> stop_thresh;
|
||||
|
||||
std::shared_ptr<pangolin::Plotter> vign_plotter;
|
||||
std::shared_ptr<pangolin::Plotter> polar_plotter;
|
||||
std::shared_ptr<pangolin::Plotter> azimuth_plotter;
|
||||
|
||||
std::vector<pangolin::Colour> cam_colors;
|
||||
|
||||
pangolin::View *img_view_display;
|
||||
|
||||
std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
|
||||
|
||||
pangolin::DataLog vign_data_log;
|
||||
|
||||
std::vector<std::shared_ptr<pangolin::DataLog>> polar_data_log,
|
||||
azimuth_data_log;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
182
include/basalt/calibration/cam_imu_calib.h
Normal file
182
include/basalt/calibration/cam_imu_calib.h
Normal file
@@ -0,0 +1,182 @@
|
||||
/**
|
||||
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 <pangolin/display/image_view.h>
|
||||
#include <pangolin/gl/gldraw.h>
|
||||
#include <pangolin/image/image.h>
|
||||
#include <pangolin/image/image_io.h>
|
||||
#include <pangolin/image/typed_image.h>
|
||||
#include <pangolin/pangolin.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <thread>
|
||||
|
||||
#include <basalt/calibration/aprilgrid.h>
|
||||
#include <basalt/calibration/calibration_helper.h>
|
||||
#include <basalt/utils/test_utils.h>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <int N, typename Scalar>
|
||||
class SplineOptimization;
|
||||
|
||||
class CamImuCalib {
|
||||
public:
|
||||
CamImuCalib(const std::string &dataset_path, const std::string &dataset_type,
|
||||
const std::string &aprilgrid_path, const std::string &cache_path,
|
||||
const std::string &cache_dataset_name, int skip_images,
|
||||
const std::vector<double> &imu_noise, bool show_gui = true);
|
||||
|
||||
~CamImuCalib();
|
||||
|
||||
void initGui();
|
||||
|
||||
void setNumCameras(size_t n);
|
||||
|
||||
void renderingLoop();
|
||||
|
||||
void computeProjections();
|
||||
|
||||
void detectCorners();
|
||||
|
||||
void initCamPoses();
|
||||
|
||||
void initCamImuTransform();
|
||||
|
||||
void initOptimization();
|
||||
|
||||
void initMocap();
|
||||
|
||||
void loadDataset();
|
||||
|
||||
void optimize();
|
||||
|
||||
bool optimizeWithParam(bool print_info,
|
||||
std::map<std::string, double> *stats = nullptr);
|
||||
|
||||
void saveCalib();
|
||||
|
||||
void saveMocapCalib();
|
||||
|
||||
void drawImageOverlay(pangolin::View &v, size_t cam_id);
|
||||
|
||||
void recomputeDataLog();
|
||||
|
||||
void drawPlots();
|
||||
|
||||
bool hasCorners() const;
|
||||
|
||||
void setOptIntrinsics(bool opt) { opt_intr = opt; }
|
||||
|
||||
private:
|
||||
static constexpr int UI_WIDTH = 300;
|
||||
|
||||
VioDatasetPtr vio_dataset;
|
||||
|
||||
CalibCornerMap calib_corners;
|
||||
CalibCornerMap calib_corners_rejected;
|
||||
CalibInitPoseMap calib_init_poses;
|
||||
|
||||
std::shared_ptr<std::thread> processing_thread;
|
||||
|
||||
std::shared_ptr<SplineOptimization<5, double>> calib_opt;
|
||||
|
||||
std::map<TimeCamId, ProjectedCornerData> reprojected_corners;
|
||||
|
||||
std::string dataset_path;
|
||||
std::string dataset_type;
|
||||
|
||||
AprilGrid april_grid;
|
||||
|
||||
std::string cache_path;
|
||||
std::string cache_dataset_name;
|
||||
|
||||
int skip_images;
|
||||
|
||||
bool show_gui;
|
||||
|
||||
const size_t MIN_CORNERS = 15;
|
||||
|
||||
std::vector<double> imu_noise;
|
||||
|
||||
//////////////////////
|
||||
|
||||
pangolin::Var<int> show_frame;
|
||||
|
||||
pangolin::Var<bool> show_corners;
|
||||
pangolin::Var<bool> show_corners_rejected;
|
||||
pangolin::Var<bool> show_init_reproj;
|
||||
pangolin::Var<bool> show_opt;
|
||||
pangolin::Var<bool> show_ids;
|
||||
|
||||
pangolin::Var<bool> show_accel;
|
||||
pangolin::Var<bool> show_gyro;
|
||||
pangolin::Var<bool> show_pos;
|
||||
pangolin::Var<bool> show_rot_error;
|
||||
|
||||
pangolin::Var<bool> show_mocap;
|
||||
pangolin::Var<bool> show_mocap_rot_error;
|
||||
pangolin::Var<bool> show_mocap_rot_vel;
|
||||
|
||||
pangolin::Var<bool> show_spline;
|
||||
pangolin::Var<bool> show_data;
|
||||
|
||||
pangolin::Var<bool> opt_intr;
|
||||
pangolin::Var<bool> opt_poses;
|
||||
pangolin::Var<bool> opt_corners;
|
||||
pangolin::Var<bool> opt_cam_time_offset;
|
||||
pangolin::Var<bool> opt_imu_scale;
|
||||
|
||||
pangolin::Var<bool> opt_mocap;
|
||||
|
||||
pangolin::Var<double> huber_thresh;
|
||||
|
||||
pangolin::Var<bool> opt_until_convg;
|
||||
pangolin::Var<double> stop_thresh;
|
||||
|
||||
pangolin::Plotter *plotter;
|
||||
pangolin::View *img_view_display;
|
||||
|
||||
std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
|
||||
|
||||
pangolin::DataLog imu_data_log, pose_data_log, mocap_data_log, vign_data_log;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
89
include/basalt/calibration/vignette.h
Normal file
89
include/basalt/calibration/vignette.h
Normal file
@@ -0,0 +1,89 @@
|
||||
/**
|
||||
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/calibration/aprilgrid.h>
|
||||
#include <basalt/calibration/calibration_helper.h>
|
||||
|
||||
#include <basalt/spline/rd_spline.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class VignetteEstimator {
|
||||
public:
|
||||
static const int SPLINE_N = 4;
|
||||
static const int64_t knot_spacing = 1e10;
|
||||
static const int border_size = 2;
|
||||
|
||||
VignetteEstimator(
|
||||
const VioDatasetPtr &vio_dataset,
|
||||
const Eigen::aligned_vector<Eigen::Vector2d> &optical_centers,
|
||||
const Eigen::aligned_vector<Eigen::Vector2i> &resolutions,
|
||||
const std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
|
||||
&reprojected_vignette,
|
||||
const AprilGrid &april_grid);
|
||||
|
||||
void compute_error(std::map<TimeCamId, std::vector<double>>
|
||||
*reprojected_vignette_error = nullptr);
|
||||
|
||||
void opt_irradience();
|
||||
|
||||
void opt_vign();
|
||||
|
||||
void optimize();
|
||||
|
||||
void compute_data_log(std::vector<std::vector<float>> &vign_data_log);
|
||||
|
||||
void save_vign_png(const std::string &path);
|
||||
|
||||
inline const std::vector<basalt::RdSpline<1, SPLINE_N>> &get_vign_param() {
|
||||
return vign_param;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
private:
|
||||
const VioDatasetPtr vio_dataset;
|
||||
Eigen::aligned_vector<Eigen::Vector2d> optical_centers;
|
||||
Eigen::aligned_vector<Eigen::Vector2i> resolutions;
|
||||
std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
|
||||
reprojected_vignette;
|
||||
const AprilGrid &april_grid;
|
||||
|
||||
size_t vign_size;
|
||||
std::vector<double> irradiance;
|
||||
std::vector<basalt::RdSpline<1, SPLINE_N>> vign_param;
|
||||
};
|
||||
} // namespace basalt
|
||||
118
include/basalt/device/rs_t265.h
Normal file
118
include/basalt/device/rs_t265.h
Normal file
@@ -0,0 +1,118 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt.git
|
||||
|
||||
Copyright (c) 2019, Vladyslav Usenko, Michael Loipführer 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 <math.h>
|
||||
#include <atomic>
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include <librealsense2/rs.hpp>
|
||||
|
||||
#include <pangolin/image/typed_image.h>
|
||||
#include <pangolin/pangolin.h>
|
||||
|
||||
#include <tbb/concurrent_queue.h>
|
||||
|
||||
#include <basalt/imu/imu_types.h>
|
||||
#include <basalt/optical_flow/optical_flow.h>
|
||||
#include <basalt/calibration/calibration.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
struct RsIMUData {
|
||||
double timestamp;
|
||||
Eigen::Vector3d data;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct RsPoseData {
|
||||
int64_t t_ns;
|
||||
Sophus::SE3d data;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
class RsT265Device {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<RsT265Device>;
|
||||
|
||||
static constexpr int IMU_RATE = 200;
|
||||
static constexpr int NUM_CAMS = 2;
|
||||
|
||||
RsT265Device(bool manual_exposure, int skip_frames, int webp_quality,
|
||||
double exposure_value = 10.0);
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
|
||||
bool setExposure(double exposure); // in milliseconds
|
||||
void setSkipFrames(int skip);
|
||||
void setWebpQuality(int quality);
|
||||
|
||||
std::shared_ptr<basalt::Calibration<double>> exportCalibration();
|
||||
|
||||
OpticalFlowInput::Ptr last_img_data;
|
||||
tbb::concurrent_bounded_queue<OpticalFlowInput::Ptr>* image_data_queue =
|
||||
nullptr;
|
||||
tbb::concurrent_bounded_queue<ImuData<double>::Ptr>* imu_data_queue = nullptr;
|
||||
tbb::concurrent_bounded_queue<RsPoseData>* pose_data_queue = nullptr;
|
||||
|
||||
private:
|
||||
bool manual_exposure;
|
||||
int skip_frames;
|
||||
int webp_quality;
|
||||
|
||||
int frame_counter = 0;
|
||||
|
||||
Eigen::aligned_deque<RsIMUData> gyro_data_queue;
|
||||
std::shared_ptr<RsIMUData> prev_accel_data;
|
||||
|
||||
std::shared_ptr<basalt::Calibration<double>> calib;
|
||||
|
||||
rs2::context context;
|
||||
rs2::config config;
|
||||
rs2::pipeline pipe;
|
||||
rs2::sensor sensor;
|
||||
|
||||
rs2::pipeline_profile profile;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
167
include/basalt/hash_bow/hash_bow.h
Normal file
167
include/basalt/hash_bow/hash_bow.h
Normal file
@@ -0,0 +1,167 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <bitset>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <basalt/utils/common_types.h>
|
||||
|
||||
#include <tbb/concurrent_unordered_map.h>
|
||||
#include <tbb/concurrent_vector.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <size_t N>
|
||||
class HashBow {
|
||||
public:
|
||||
HashBow(size_t num_bits) : num_bits(num_bits < 32 ? num_bits : 32) {
|
||||
static_assert(N < 512,
|
||||
"This implementation of HashBow only supports the descriptor "
|
||||
"length below 512.");
|
||||
}
|
||||
|
||||
inline FeatureHash compute_hash(const std::bitset<N>& descriptor) const {
|
||||
FeatureHash res;
|
||||
for (size_t i = 0; i < num_bits; ++i) {
|
||||
res[i] = descriptor[word_bit_permutation[i]];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
inline void compute_bow(const std::vector<std::bitset<N>>& descriptors,
|
||||
std::vector<FeatureHash>& hashes,
|
||||
HashBowVector& bow_vector) const {
|
||||
size_t descriptors_size = descriptors.size();
|
||||
hashes.resize(descriptors_size);
|
||||
|
||||
std::unordered_map<FeatureHash, double> bow_map;
|
||||
bow_map.clear();
|
||||
bow_map.reserve(descriptors_size);
|
||||
|
||||
for (size_t i = 0; i < descriptors_size; i++) {
|
||||
hashes[i] = compute_hash(descriptors[i]);
|
||||
bow_map[hashes[i]] += 1.0;
|
||||
}
|
||||
|
||||
bow_vector.clear();
|
||||
|
||||
double l1_sum = 0;
|
||||
for (const auto& kv : bow_map) {
|
||||
bow_vector.emplace_back(kv);
|
||||
l1_sum += std::abs(kv.second);
|
||||
}
|
||||
|
||||
for (auto& kv : bow_vector) {
|
||||
kv.second /= l1_sum;
|
||||
}
|
||||
}
|
||||
|
||||
inline void add_to_database(const TimeCamId& tcid,
|
||||
const HashBowVector& bow_vector) {
|
||||
for (const auto& kv : bow_vector) {
|
||||
// std::pair<TimeCamId, double> p = std::make_pair(tcid, kv.second);
|
||||
inverted_index[kv.first].emplace_back(tcid, kv.second);
|
||||
}
|
||||
}
|
||||
|
||||
inline void querry_database(
|
||||
const HashBowVector& bow_vector, size_t num_results,
|
||||
std::vector<std::pair<TimeCamId, double>>& results,
|
||||
const int64_t* max_t_ns = nullptr) const {
|
||||
results.clear();
|
||||
|
||||
std::unordered_map<TimeCamId, double> scores;
|
||||
|
||||
for (const auto& kv : bow_vector) {
|
||||
const auto range_it = inverted_index.find(kv.first);
|
||||
|
||||
if (range_it != inverted_index.end())
|
||||
for (const auto& v : range_it->second) {
|
||||
// if there is a maximum query time select only the frames that have
|
||||
// timestamp below max_t_ns
|
||||
if (!max_t_ns || v.first.frame_id < (*max_t_ns))
|
||||
scores[v.first] += std::abs(kv.second - v.second) -
|
||||
std::abs(kv.second) - std::abs(v.second);
|
||||
}
|
||||
}
|
||||
|
||||
results.reserve(scores.size());
|
||||
|
||||
for (const auto& kv : scores)
|
||||
results.emplace_back(kv.first, -kv.second / 2.0);
|
||||
|
||||
if (results.size() > num_results) {
|
||||
std::partial_sort(
|
||||
results.begin(), results.begin() + num_results, results.end(),
|
||||
[](const auto& a, const auto& b) { return a.second > b.second; });
|
||||
|
||||
results.resize(num_results);
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
constexpr static const size_t random_bit_permutation[512] = {
|
||||
484, 458, 288, 170, 215, 424, 41, 38, 293, 96, 172, 428, 508, 52, 370,
|
||||
1, 182, 472, 89, 339, 273, 234, 98, 217, 73, 195, 307, 306, 113, 429,
|
||||
161, 443, 364, 439, 301, 247, 325, 24, 490, 366, 75, 7, 464, 232, 49,
|
||||
196, 144, 69, 470, 387, 3, 86, 361, 313, 396, 356, 94, 201, 291, 360,
|
||||
107, 251, 413, 393, 296, 124, 308, 146, 298, 160, 121, 302, 151, 345, 336,
|
||||
26, 63, 238, 79, 267, 262, 437, 433, 350, 53, 134, 194, 452, 114, 54,
|
||||
82, 214, 191, 242, 482, 37, 432, 311, 130, 460, 422, 221, 271, 192, 474,
|
||||
46, 289, 34, 20, 95, 463, 499, 159, 272, 481, 129, 448, 173, 323, 258,
|
||||
416, 229, 334, 510, 461, 263, 362, 346, 39, 500, 381, 401, 492, 299, 33,
|
||||
169, 241, 11, 254, 449, 199, 486, 400, 365, 70, 436, 108, 19, 233, 505,
|
||||
152, 6, 480, 468, 278, 426, 253, 471, 328, 327, 139, 29, 27, 488, 332,
|
||||
290, 412, 164, 259, 352, 222, 186, 32, 319, 410, 211, 405, 187, 213, 507,
|
||||
205, 395, 62, 178, 36, 140, 87, 491, 351, 450, 314, 77, 342, 132, 133,
|
||||
477, 103, 389, 206, 197, 324, 485, 425, 297, 231, 123, 447, 126, 9, 64,
|
||||
181, 40, 14, 5, 261, 431, 333, 223, 4, 138, 220, 76, 44, 300, 331,
|
||||
78, 193, 497, 403, 435, 275, 147, 66, 368, 141, 451, 225, 250, 61, 18,
|
||||
444, 208, 380, 109, 255, 337, 372, 212, 359, 457, 31, 398, 354, 219, 117,
|
||||
248, 392, 203, 88, 479, 509, 149, 120, 145, 51, 15, 367, 190, 163, 417,
|
||||
454, 329, 183, 390, 83, 404, 249, 81, 264, 445, 317, 179, 244, 473, 71,
|
||||
111, 118, 209, 171, 224, 459, 446, 104, 13, 377, 200, 414, 198, 420, 226,
|
||||
153, 384, 25, 441, 305, 338, 316, 483, 184, 402, 48, 131, 502, 252, 469,
|
||||
12, 167, 243, 373, 35, 127, 341, 455, 379, 210, 340, 128, 430, 57, 434,
|
||||
330, 415, 494, 142, 355, 282, 322, 65, 105, 421, 68, 409, 466, 245, 59,
|
||||
269, 112, 386, 257, 256, 93, 174, 16, 60, 143, 343, 115, 506, 276, 10,
|
||||
496, 489, 235, 47, 136, 22, 165, 204, 42, 465, 440, 498, 312, 504, 116,
|
||||
419, 185, 303, 218, 353, 283, 374, 2, 177, 137, 240, 102, 309, 292, 85,
|
||||
453, 388, 397, 438, 281, 279, 442, 110, 55, 101, 100, 150, 375, 406, 157,
|
||||
23, 0, 237, 376, 236, 216, 8, 154, 91, 456, 423, 176, 427, 284, 30,
|
||||
84, 349, 335, 56, 270, 227, 286, 168, 239, 122, 478, 162, 475, 166, 17,
|
||||
348, 285, 175, 155, 266, 382, 304, 268, 180, 295, 125, 371, 467, 277, 294,
|
||||
58, 347, 72, 280, 50, 287, 511, 80, 260, 326, 495, 45, 106, 399, 369,
|
||||
503, 357, 315, 418, 487, 99, 43, 320, 188, 407, 246, 501, 119, 158, 274,
|
||||
408, 230, 358, 90, 148, 363, 207, 344, 265, 462, 189, 310, 385, 67, 28,
|
||||
383, 378, 156, 394, 97, 476, 493, 321, 411, 228, 21, 391, 202, 92, 318,
|
||||
74, 135};
|
||||
|
||||
constexpr static std::array<size_t, FEATURE_HASH_MAX_SIZE>
|
||||
compute_permutation() {
|
||||
std::array<size_t, FEATURE_HASH_MAX_SIZE> res{};
|
||||
size_t j = 0;
|
||||
for (size_t i = 0; i < 512 && j < FEATURE_HASH_MAX_SIZE; ++i) {
|
||||
if (random_bit_permutation[i] < N) {
|
||||
res[j] = random_bit_permutation[i];
|
||||
j++;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
constexpr static const std::array<size_t, FEATURE_HASH_MAX_SIZE>
|
||||
word_bit_permutation = compute_permutation();
|
||||
|
||||
size_t num_bits;
|
||||
|
||||
tbb::concurrent_unordered_map<
|
||||
FeatureHash, tbb::concurrent_vector<std::pair<TimeCamId, double>>,
|
||||
std::hash<FeatureHash>>
|
||||
inverted_index;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
178
include/basalt/io/dataset_io.h
Normal file
178
include/basalt/io/dataset_io.h
Normal file
@@ -0,0 +1,178 @@
|
||||
/**
|
||||
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 <array>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <cereal/archives/binary.hpp>
|
||||
#include <cereal/archives/json.hpp>
|
||||
#include <cereal/types/bitset.hpp>
|
||||
#include <cereal/types/deque.hpp>
|
||||
#include <cereal/types/map.hpp>
|
||||
#include <cereal/types/memory.hpp>
|
||||
#include <cereal/types/polymorphic.hpp>
|
||||
#include <cereal/types/set.hpp>
|
||||
#include <cereal/types/string.hpp>
|
||||
#include <cereal/types/unordered_map.hpp>
|
||||
#include <cereal/types/utility.hpp>
|
||||
#include <cereal/types/vector.hpp>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
#include <basalt/image/image.h>
|
||||
#include <basalt/utils/assert.h>
|
||||
|
||||
#include <basalt/camera/generic_camera.hpp>
|
||||
#include <basalt/camera/stereographic_param.hpp>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
struct ImageData {
|
||||
ImageData() : exposure(0) {}
|
||||
|
||||
ManagedImage<uint16_t>::Ptr img;
|
||||
double exposure;
|
||||
};
|
||||
|
||||
struct Observations {
|
||||
Eigen::aligned_vector<Eigen::Vector2d> pos;
|
||||
std::vector<int> id;
|
||||
};
|
||||
|
||||
struct GyroData {
|
||||
int64_t timestamp_ns;
|
||||
Eigen::Vector3d data;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct AccelData {
|
||||
int64_t timestamp_ns;
|
||||
Eigen::Vector3d data;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct PoseData {
|
||||
int64_t timestamp_ns;
|
||||
Sophus::SE3d data;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct MocapPoseData {
|
||||
int64_t timestamp_ns;
|
||||
Sophus::SE3d data;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct AprilgridCornersData {
|
||||
int64_t timestamp_ns;
|
||||
int cam_id;
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector2d> corner_pos;
|
||||
std::vector<int> corner_id;
|
||||
};
|
||||
|
||||
class VioDataset {
|
||||
public:
|
||||
virtual ~VioDataset(){};
|
||||
|
||||
virtual size_t get_num_cams() const = 0;
|
||||
|
||||
virtual std::vector<int64_t> &get_image_timestamps() = 0;
|
||||
|
||||
virtual const Eigen::aligned_vector<AccelData> &get_accel_data() const = 0;
|
||||
virtual const Eigen::aligned_vector<GyroData> &get_gyro_data() const = 0;
|
||||
virtual const std::vector<int64_t> &get_gt_timestamps() const = 0;
|
||||
virtual const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data()
|
||||
const = 0;
|
||||
virtual int64_t get_mocap_to_imu_offset_ns() const = 0;
|
||||
virtual std::vector<ImageData> get_image_data(int64_t t_ns) = 0;
|
||||
virtual std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) = 0;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<VioDataset> VioDatasetPtr;
|
||||
|
||||
class DatasetIoInterface {
|
||||
public:
|
||||
virtual void read(const std::string &path) = 0;
|
||||
virtual void reset() = 0;
|
||||
virtual VioDatasetPtr get_data() = 0;
|
||||
|
||||
virtual ~DatasetIoInterface(){};
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<DatasetIoInterface> DatasetIoInterfacePtr;
|
||||
|
||||
class DatasetIoFactory {
|
||||
public:
|
||||
static DatasetIoInterfacePtr getDatasetIo(const std::string &dataset_type,
|
||||
bool load_mocap_as_gt = false);
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
namespace cereal {
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive &archive, basalt::ManagedImage<uint8_t> &m) {
|
||||
archive(m.w);
|
||||
archive(m.h);
|
||||
|
||||
m.Reinitialise(m.w, m.h);
|
||||
|
||||
archive(binary_data(m.ptr, m.size()));
|
||||
}
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive &ar, basalt::GyroData &c) {
|
||||
ar(c.timestamp_ns, c.data);
|
||||
}
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive &ar, basalt::AccelData &c) {
|
||||
ar(c.timestamp_ns, c.data);
|
||||
}
|
||||
|
||||
} // namespace cereal
|
||||
336
include/basalt/io/dataset_io_euroc.h
Normal file
336
include/basalt/io/dataset_io_euroc.h
Normal file
@@ -0,0 +1,336 @@
|
||||
/**
|
||||
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 DATASET_IO_EUROC_H
|
||||
#define DATASET_IO_EUROC_H
|
||||
|
||||
#include <basalt/io/dataset_io.h>
|
||||
#include <basalt/utils/filesystem.h>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class EurocVioDataset : public VioDataset {
|
||||
size_t num_cams;
|
||||
|
||||
std::string path;
|
||||
|
||||
std::vector<int64_t> image_timestamps;
|
||||
std::unordered_map<int64_t, std::string> image_path;
|
||||
|
||||
// vector of images for every timestamp
|
||||
// assumes vectors size is num_cams for every timestamp with null pointers for
|
||||
// missing frames
|
||||
// std::unordered_map<int64_t, std::vector<ImageData>> image_data;
|
||||
|
||||
Eigen::aligned_vector<AccelData> accel_data;
|
||||
Eigen::aligned_vector<GyroData> gyro_data;
|
||||
|
||||
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
|
||||
Eigen::aligned_vector<Sophus::SE3d>
|
||||
gt_pose_data; // TODO: change to eigen aligned
|
||||
|
||||
int64_t mocap_to_imu_offset_ns = 0;
|
||||
|
||||
std::vector<std::unordered_map<int64_t, double>> exposure_times;
|
||||
|
||||
public:
|
||||
~EurocVioDataset(){};
|
||||
|
||||
size_t get_num_cams() const { return num_cams; }
|
||||
|
||||
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
|
||||
|
||||
const Eigen::aligned_vector<AccelData> &get_accel_data() const {
|
||||
return accel_data;
|
||||
}
|
||||
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
|
||||
return gyro_data;
|
||||
}
|
||||
const std::vector<int64_t> &get_gt_timestamps() const {
|
||||
return gt_timestamps;
|
||||
}
|
||||
const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
|
||||
return gt_pose_data;
|
||||
}
|
||||
|
||||
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
|
||||
|
||||
std::vector<ImageData> get_image_data(int64_t t_ns) {
|
||||
std::vector<ImageData> res(num_cams);
|
||||
|
||||
const std::vector<std::string> folder = {"/mav0/cam0/", "/mav0/cam1/"};
|
||||
|
||||
for (size_t i = 0; i < num_cams; i++) {
|
||||
std::string full_image_path =
|
||||
path + folder[i] + "data/" + image_path[t_ns];
|
||||
|
||||
if (fs::exists(full_image_path)) {
|
||||
cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
|
||||
|
||||
if (img.type() == CV_8UC1) {
|
||||
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
|
||||
|
||||
const uint8_t *data_in = img.ptr();
|
||||
uint16_t *data_out = res[i].img->ptr;
|
||||
|
||||
size_t full_size = img.cols * img.rows;
|
||||
for (size_t i = 0; i < full_size; i++) {
|
||||
int val = data_in[i];
|
||||
val = val << 8;
|
||||
data_out[i] = val;
|
||||
}
|
||||
} else if (img.type() == CV_8UC3) {
|
||||
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
|
||||
|
||||
const uint8_t *data_in = img.ptr();
|
||||
uint16_t *data_out = res[i].img->ptr;
|
||||
|
||||
size_t full_size = img.cols * img.rows;
|
||||
for (size_t i = 0; i < full_size; i++) {
|
||||
int val = data_in[i * 3];
|
||||
val = val << 8;
|
||||
data_out[i] = val;
|
||||
}
|
||||
} else if (img.type() == CV_16UC1) {
|
||||
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
|
||||
std::memcpy(res[i].img->ptr, img.ptr(),
|
||||
img.cols * img.rows * sizeof(uint16_t));
|
||||
|
||||
} else {
|
||||
std::cerr << "img.fmt.bpp " << img.type() << std::endl;
|
||||
std::abort();
|
||||
}
|
||||
|
||||
auto exp_it = exposure_times[i].find(t_ns);
|
||||
if (exp_it != exposure_times[i].end()) {
|
||||
res[i].exposure = exp_it->second;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
// returns a vector of 2 images: for the first and the second camera at the defined timestamp.
|
||||
std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
|
||||
std::vector<cv::Mat> res(num_cams);
|
||||
|
||||
const std::vector<std::string> folder = {"/mav0/cam0/", "/mav0/cam1/"};
|
||||
|
||||
for (size_t i = 0; i < num_cams; i++) {
|
||||
std::string full_image_path =
|
||||
path + folder[i] + "data/" + image_path[t_ns];
|
||||
|
||||
if (fs::exists(full_image_path)) {
|
||||
cv::Mat img = cv::imread(full_image_path, cv::IMREAD_COLOR);
|
||||
|
||||
res.push_back(img);
|
||||
// The rest of the body from get_image_data() was deleted.
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
friend class EurocIO;
|
||||
};
|
||||
|
||||
class EurocIO : public DatasetIoInterface {
|
||||
public:
|
||||
EurocIO(bool load_mocap_as_gt) : load_mocap_as_gt(load_mocap_as_gt) {}
|
||||
|
||||
void read(const std::string &path) {
|
||||
if (!fs::exists(path))
|
||||
std::cerr << "No dataset found in " << path << std::endl;
|
||||
|
||||
data.reset(new EurocVioDataset);
|
||||
|
||||
data->num_cams = 2;
|
||||
data->path = path;
|
||||
|
||||
read_image_timestamps(path + "/mav0/cam0/");
|
||||
|
||||
read_imu_data(path + "/mav0/imu0/");
|
||||
|
||||
if (!load_mocap_as_gt &&
|
||||
fs::exists(path + "/mav0/state_groundtruth_estimate0/data.csv")) {
|
||||
read_gt_data_state(path + "/mav0/state_groundtruth_estimate0/");
|
||||
} else if (!load_mocap_as_gt && fs::exists(path + "/mav0/gt/data.csv")) {
|
||||
read_gt_data_pose(path + "/mav0/gt/");
|
||||
} else if (fs::exists(path + "/mav0/mocap0/data.csv")) {
|
||||
read_gt_data_pose(path + "/mav0/mocap0/");
|
||||
}
|
||||
|
||||
data->exposure_times.resize(data->num_cams);
|
||||
if (fs::exists(path + "/mav0/cam0/exposure.csv")) {
|
||||
std::cout << "Loading exposure times for cam0" << std::endl;
|
||||
read_exposure(path + "/mav0/cam0/", data->exposure_times[0]);
|
||||
}
|
||||
if (fs::exists(path + "/mav0/cam1/exposure.csv")) {
|
||||
std::cout << "Loading exposure times for cam1" << std::endl;
|
||||
read_exposure(path + "/mav0/cam1/", data->exposure_times[1]);
|
||||
}
|
||||
}
|
||||
|
||||
void reset() { data.reset(); }
|
||||
|
||||
VioDatasetPtr get_data() { return data; }
|
||||
|
||||
private:
|
||||
void read_exposure(const std::string &path,
|
||||
std::unordered_map<int64_t, double> &exposure_data) {
|
||||
exposure_data.clear();
|
||||
|
||||
std::ifstream f(path + "exposure.csv");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
char tmp;
|
||||
int64_t timestamp, exposure_int;
|
||||
Eigen::Vector3d gyro, accel;
|
||||
|
||||
ss >> timestamp >> tmp >> exposure_int;
|
||||
|
||||
exposure_data[timestamp] = exposure_int * 1e-9;
|
||||
}
|
||||
}
|
||||
|
||||
void read_image_timestamps(const std::string &path) {
|
||||
std::ifstream f(path + "data.csv");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
std::stringstream ss(line);
|
||||
char tmp;
|
||||
int64_t t_ns;
|
||||
std::string path;
|
||||
ss >> t_ns >> tmp >> path;
|
||||
|
||||
data->image_timestamps.emplace_back(t_ns);
|
||||
data->image_path[t_ns] = path;
|
||||
}
|
||||
}
|
||||
|
||||
void read_imu_data(const std::string &path) {
|
||||
data->accel_data.clear();
|
||||
data->gyro_data.clear();
|
||||
|
||||
std::ifstream f(path + "data.csv");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
char tmp;
|
||||
uint64_t timestamp;
|
||||
Eigen::Vector3d gyro, accel;
|
||||
|
||||
ss >> timestamp >> tmp >> gyro[0] >> tmp >> gyro[1] >> tmp >> gyro[2] >>
|
||||
tmp >> accel[0] >> tmp >> accel[1] >> tmp >> accel[2];
|
||||
|
||||
data->accel_data.emplace_back();
|
||||
data->accel_data.back().timestamp_ns = timestamp;
|
||||
data->accel_data.back().data = accel;
|
||||
|
||||
data->gyro_data.emplace_back();
|
||||
data->gyro_data.back().timestamp_ns = timestamp;
|
||||
data->gyro_data.back().data = gyro;
|
||||
}
|
||||
}
|
||||
|
||||
void read_gt_data_state(const std::string &path) {
|
||||
data->gt_timestamps.clear();
|
||||
data->gt_pose_data.clear();
|
||||
|
||||
std::ifstream f(path + "data.csv");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
char tmp;
|
||||
uint64_t timestamp;
|
||||
Eigen::Quaterniond q;
|
||||
Eigen::Vector3d pos, vel, accel_bias, gyro_bias;
|
||||
|
||||
ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >>
|
||||
tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z() >> tmp >>
|
||||
vel[0] >> tmp >> vel[1] >> tmp >> vel[2] >> tmp >> accel_bias[0] >>
|
||||
tmp >> accel_bias[1] >> tmp >> accel_bias[2] >> tmp >> gyro_bias[0] >>
|
||||
tmp >> gyro_bias[1] >> tmp >> gyro_bias[2];
|
||||
|
||||
data->gt_timestamps.emplace_back(timestamp);
|
||||
data->gt_pose_data.emplace_back(q, pos);
|
||||
}
|
||||
}
|
||||
|
||||
void read_gt_data_pose(const std::string &path) {
|
||||
data->gt_timestamps.clear();
|
||||
data->gt_pose_data.clear();
|
||||
|
||||
std::ifstream f(path + "data.csv");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
char tmp;
|
||||
uint64_t timestamp;
|
||||
Eigen::Quaterniond q;
|
||||
Eigen::Vector3d pos;
|
||||
|
||||
ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >>
|
||||
tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z();
|
||||
|
||||
data->gt_timestamps.emplace_back(timestamp);
|
||||
data->gt_pose_data.emplace_back(q, pos);
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<EurocVioDataset> data;
|
||||
bool load_mocap_as_gt;
|
||||
}; // namespace basalt
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
#endif // DATASET_IO_H
|
||||
220
include/basalt/io/dataset_io_kitti.h
Normal file
220
include/basalt/io/dataset_io_kitti.h
Normal file
@@ -0,0 +1,220 @@
|
||||
/**
|
||||
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/io/dataset_io.h>
|
||||
#include <basalt/utils/filesystem.h>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class KittiVioDataset : public VioDataset {
|
||||
size_t num_cams;
|
||||
|
||||
std::string path;
|
||||
|
||||
std::vector<int64_t> image_timestamps;
|
||||
std::unordered_map<int64_t, std::string> image_path;
|
||||
|
||||
// vector of images for every timestamp
|
||||
// assumes vectors size is num_cams for every timestamp with null pointers for
|
||||
// missing frames
|
||||
// std::unordered_map<int64_t, std::vector<ImageData>> image_data;
|
||||
|
||||
Eigen::aligned_vector<AccelData> accel_data;
|
||||
Eigen::aligned_vector<GyroData> gyro_data;
|
||||
|
||||
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
|
||||
Eigen::aligned_vector<Sophus::SE3d>
|
||||
gt_pose_data; // TODO: change to eigen aligned
|
||||
|
||||
int64_t mocap_to_imu_offset_ns;
|
||||
|
||||
public:
|
||||
~KittiVioDataset(){};
|
||||
|
||||
size_t get_num_cams() const { return num_cams; }
|
||||
|
||||
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
|
||||
|
||||
const Eigen::aligned_vector<AccelData> &get_accel_data() const {
|
||||
return accel_data;
|
||||
}
|
||||
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
|
||||
return gyro_data;
|
||||
}
|
||||
const std::vector<int64_t> &get_gt_timestamps() const {
|
||||
return gt_timestamps;
|
||||
}
|
||||
const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
|
||||
return gt_pose_data;
|
||||
}
|
||||
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
|
||||
|
||||
std::vector<ImageData> get_image_data(int64_t t_ns) {
|
||||
std::vector<ImageData> res(num_cams);
|
||||
|
||||
const std::vector<std::string> folder = {"/image_0/", "/image_1/"};
|
||||
|
||||
for (size_t i = 0; i < num_cams; i++) {
|
||||
std::string full_image_path = path + folder[i] + image_path[t_ns];
|
||||
|
||||
if (fs::exists(full_image_path)) {
|
||||
cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
|
||||
|
||||
if (img.type() == CV_8UC1) {
|
||||
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
|
||||
|
||||
const uint8_t *data_in = img.ptr();
|
||||
uint16_t *data_out = res[i].img->ptr;
|
||||
|
||||
size_t full_size = img.cols * img.rows;
|
||||
for (size_t i = 0; i < full_size; i++) {
|
||||
int val = data_in[i];
|
||||
val = val << 8;
|
||||
data_out[i] = val;
|
||||
}
|
||||
} else {
|
||||
std::cerr << "img.fmt.bpp " << img.type() << std::endl;
|
||||
std::abort();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
|
||||
std::vector<cv::Mat> res(num_cams);
|
||||
|
||||
const std::vector<std::string> folder = {"/image_0/", "/image_1/"};
|
||||
|
||||
for (size_t i = 0; i < num_cams; i++) {
|
||||
std::string full_image_path =
|
||||
path + folder[i] + image_path[t_ns];
|
||||
|
||||
if (fs::exists(full_image_path)) {
|
||||
cv::Mat img = cv::imread(full_image_path, cv::IMREAD_GRAYSCALE);
|
||||
|
||||
res.push_back(img);
|
||||
// The rest of the body from get_image_data() was deleted.
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
friend class KittiIO;
|
||||
};
|
||||
|
||||
class KittiIO : public DatasetIoInterface {
|
||||
public:
|
||||
KittiIO() {}
|
||||
|
||||
void read(const std::string &path) {
|
||||
if (!fs::exists(path))
|
||||
std::cerr << "No dataset found in " << path << std::endl;
|
||||
|
||||
data.reset(new KittiVioDataset);
|
||||
|
||||
data->num_cams = 2;
|
||||
data->path = path;
|
||||
|
||||
read_image_timestamps(path + "/times.txt");
|
||||
|
||||
if (fs::exists(path + "/poses.txt")) {
|
||||
read_gt_data_pose(path + "/poses.txt");
|
||||
}
|
||||
}
|
||||
|
||||
void reset() { data.reset(); }
|
||||
|
||||
VioDatasetPtr get_data() { return data; }
|
||||
|
||||
private:
|
||||
void read_image_timestamps(const std::string &path) {
|
||||
std::ifstream f(path);
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
std::stringstream ss(line);
|
||||
|
||||
double t_s;
|
||||
ss >> t_s;
|
||||
|
||||
int64_t t_ns = t_s * 1e9;
|
||||
|
||||
std::stringstream ss1;
|
||||
ss1 << std::setfill('0') << std::setw(6) << data->image_timestamps.size()
|
||||
<< ".png";
|
||||
|
||||
data->image_timestamps.emplace_back(t_ns);
|
||||
data->image_path[t_ns] = ss1.str();
|
||||
}
|
||||
}
|
||||
|
||||
void read_gt_data_pose(const std::string &path) {
|
||||
data->gt_timestamps.clear();
|
||||
data->gt_pose_data.clear();
|
||||
|
||||
int i = 0;
|
||||
|
||||
std::ifstream f(path);
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
Eigen::Matrix3d rot;
|
||||
Eigen::Vector3d pos;
|
||||
|
||||
ss >> rot(0, 0) >> rot(0, 1) >> rot(0, 2) >> pos[0] >> rot(1, 0) >>
|
||||
rot(1, 1) >> rot(1, 2) >> pos[1] >> rot(2, 0) >> rot(2, 1) >>
|
||||
rot(2, 2) >> pos[2];
|
||||
|
||||
data->gt_timestamps.emplace_back(data->image_timestamps[i]);
|
||||
data->gt_pose_data.emplace_back(Eigen::Quaterniond(rot), pos);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<KittiVioDataset> data;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
406
include/basalt/io/dataset_io_rosbag.h
Normal file
406
include/basalt/io/dataset_io_rosbag.h
Normal file
@@ -0,0 +1,406 @@
|
||||
/**
|
||||
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 DATASET_IO_ROSBAG_H
|
||||
#define DATASET_IO_ROSBAG_H
|
||||
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
|
||||
#include <basalt/io/dataset_io.h>
|
||||
|
||||
// Hack to access private functions
|
||||
#define private public
|
||||
#include <rosbag/bag.h>
|
||||
#include <rosbag/view.h>
|
||||
#undef private
|
||||
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
|
||||
#include <basalt/utils/filesystem.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class RosbagVioDataset : public VioDataset {
|
||||
std::shared_ptr<rosbag::Bag> bag;
|
||||
std::mutex m;
|
||||
|
||||
size_t num_cams;
|
||||
|
||||
std::vector<int64_t> image_timestamps;
|
||||
|
||||
// vector of images for every timestamp
|
||||
// assumes vectors size is num_cams for every timestamp with null pointers for
|
||||
// missing frames
|
||||
std::unordered_map<int64_t, std::vector<std::optional<rosbag::IndexEntry>>>
|
||||
image_data_idx;
|
||||
|
||||
Eigen::aligned_vector<AccelData> accel_data;
|
||||
Eigen::aligned_vector<GyroData> gyro_data;
|
||||
|
||||
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
|
||||
Eigen::aligned_vector<Sophus::SE3d>
|
||||
gt_pose_data; // TODO: change to eigen aligned
|
||||
|
||||
int64_t mocap_to_imu_offset_ns;
|
||||
|
||||
public:
|
||||
~RosbagVioDataset() {}
|
||||
|
||||
size_t get_num_cams() const { return num_cams; }
|
||||
|
||||
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
|
||||
|
||||
const Eigen::aligned_vector<AccelData> &get_accel_data() const {
|
||||
return accel_data;
|
||||
}
|
||||
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
|
||||
return gyro_data;
|
||||
}
|
||||
const std::vector<int64_t> &get_gt_timestamps() const {
|
||||
return gt_timestamps;
|
||||
}
|
||||
const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
|
||||
return gt_pose_data;
|
||||
}
|
||||
|
||||
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
|
||||
|
||||
std::vector<ImageData> get_image_data(int64_t t_ns) {
|
||||
std::vector<ImageData> res(num_cams);
|
||||
|
||||
auto it = image_data_idx.find(t_ns);
|
||||
|
||||
if (it != image_data_idx.end())
|
||||
for (size_t i = 0; i < num_cams; i++) {
|
||||
ImageData &id = res[i];
|
||||
|
||||
if (!it->second[i].has_value()) continue;
|
||||
|
||||
m.lock();
|
||||
sensor_msgs::ImageConstPtr img_msg =
|
||||
bag->instantiateBuffer<sensor_msgs::Image>(*it->second[i]);
|
||||
m.unlock();
|
||||
|
||||
// std::cerr << "img_msg->width " << img_msg->width << "
|
||||
// img_msg->height "
|
||||
// << img_msg->height << std::endl;
|
||||
|
||||
id.img.reset(
|
||||
new ManagedImage<uint16_t>(img_msg->width, img_msg->height));
|
||||
|
||||
if (!img_msg->header.frame_id.empty() &&
|
||||
std::isdigit(img_msg->header.frame_id[0])) {
|
||||
id.exposure = std::stol(img_msg->header.frame_id) * 1e-9;
|
||||
} else {
|
||||
id.exposure = -1;
|
||||
}
|
||||
|
||||
if (img_msg->encoding == "mono8") {
|
||||
const uint8_t *data_in = img_msg->data.data();
|
||||
uint16_t *data_out = id.img->ptr;
|
||||
|
||||
for (size_t i = 0; i < img_msg->data.size(); i++) {
|
||||
int val = data_in[i];
|
||||
val = val << 8;
|
||||
data_out[i] = val;
|
||||
}
|
||||
|
||||
} else if (img_msg->encoding == "mono16") {
|
||||
std::memcpy(id.img->ptr, img_msg->data.data(), img_msg->data.size());
|
||||
} else {
|
||||
std::cerr << "Encoding " << img_msg->encoding << " is not supported."
|
||||
<< std::endl;
|
||||
std::abort();
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
|
||||
cv::Mat mat = cv::Mat(1, 10, CV_32F, 1);
|
||||
return mat;
|
||||
}
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
friend class RosbagIO;
|
||||
};
|
||||
|
||||
class RosbagIO : public DatasetIoInterface {
|
||||
public:
|
||||
RosbagIO() {}
|
||||
|
||||
void read(const std::string &path) {
|
||||
if (!fs::exists(path))
|
||||
std::cerr << "No dataset found in " << path << std::endl;
|
||||
|
||||
data.reset(new RosbagVioDataset);
|
||||
|
||||
data->bag.reset(new rosbag::Bag);
|
||||
data->bag->open(path, rosbag::bagmode::Read);
|
||||
|
||||
rosbag::View view(*data->bag);
|
||||
|
||||
// get topics
|
||||
std::vector<const rosbag::ConnectionInfo *> connection_infos =
|
||||
view.getConnections();
|
||||
|
||||
std::set<std::string> cam_topics;
|
||||
std::string imu_topic;
|
||||
std::string mocap_topic;
|
||||
std::string point_topic;
|
||||
|
||||
for (const rosbag::ConnectionInfo *info : connection_infos) {
|
||||
// if (info->topic.substr(0, 4) == std::string("/cam")) {
|
||||
// cam_topics.insert(info->topic);
|
||||
// } else if (info->topic.substr(0, 4) == std::string("/imu")) {
|
||||
// imu_topic = info->topic;
|
||||
// } else if (info->topic.substr(0, 5) == std::string("/vrpn") ||
|
||||
// info->topic.substr(0, 6) == std::string("/vicon")) {
|
||||
// mocap_topic = info->topic;
|
||||
// }
|
||||
|
||||
if (info->datatype == std::string("sensor_msgs/Image")) {
|
||||
cam_topics.insert(info->topic);
|
||||
} else if (info->datatype == std::string("sensor_msgs/Imu") &&
|
||||
info->topic.rfind("/fcu", 0) != 0) {
|
||||
imu_topic = info->topic;
|
||||
} else if (info->datatype ==
|
||||
std::string("geometry_msgs/TransformStamped") ||
|
||||
info->datatype == std::string("geometry_msgs/PoseStamped")) {
|
||||
mocap_topic = info->topic;
|
||||
} else if (info->datatype == std::string("geometry_msgs/PointStamped")) {
|
||||
point_topic = info->topic;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "imu_topic: " << imu_topic << std::endl;
|
||||
std::cout << "mocap_topic: " << mocap_topic << std::endl;
|
||||
std::cout << "cam_topics: ";
|
||||
for (const std::string &s : cam_topics) std::cout << s << " ";
|
||||
std::cout << std::endl;
|
||||
|
||||
std::map<std::string, int> topic_to_id;
|
||||
int idx = 0;
|
||||
for (const std::string &s : cam_topics) {
|
||||
topic_to_id[s] = idx;
|
||||
idx++;
|
||||
}
|
||||
|
||||
data->num_cams = cam_topics.size();
|
||||
|
||||
int num_msgs = 0;
|
||||
|
||||
int64_t min_time = std::numeric_limits<int64_t>::max();
|
||||
int64_t max_time = std::numeric_limits<int64_t>::min();
|
||||
|
||||
std::vector<geometry_msgs::TransformStampedConstPtr> mocap_msgs;
|
||||
std::vector<geometry_msgs::PointStampedConstPtr> point_msgs;
|
||||
|
||||
std::vector<int64_t>
|
||||
system_to_imu_offset_vec; // t_imu = t_system + system_to_imu_offset
|
||||
std::vector<int64_t> system_to_mocap_offset_vec; // t_mocap = t_system +
|
||||
// system_to_mocap_offset
|
||||
|
||||
std::set<int64_t> image_timestamps;
|
||||
|
||||
for (const rosbag::MessageInstance &m : view) {
|
||||
const std::string &topic = m.getTopic();
|
||||
|
||||
if (cam_topics.find(topic) != cam_topics.end()) {
|
||||
sensor_msgs::ImageConstPtr img_msg =
|
||||
m.instantiate<sensor_msgs::Image>();
|
||||
int64_t timestamp_ns = img_msg->header.stamp.toNSec();
|
||||
|
||||
auto &img_vec = data->image_data_idx[timestamp_ns];
|
||||
if (img_vec.size() == 0) img_vec.resize(data->num_cams);
|
||||
|
||||
img_vec[topic_to_id.at(topic)] = m.index_entry_;
|
||||
image_timestamps.insert(timestamp_ns);
|
||||
|
||||
min_time = std::min(min_time, timestamp_ns);
|
||||
max_time = std::max(max_time, timestamp_ns);
|
||||
}
|
||||
|
||||
if (imu_topic == topic) {
|
||||
sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
|
||||
int64_t time = imu_msg->header.stamp.toNSec();
|
||||
|
||||
data->accel_data.emplace_back();
|
||||
data->accel_data.back().timestamp_ns = time;
|
||||
data->accel_data.back().data = Eigen::Vector3d(
|
||||
imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y,
|
||||
imu_msg->linear_acceleration.z);
|
||||
|
||||
data->gyro_data.emplace_back();
|
||||
data->gyro_data.back().timestamp_ns = time;
|
||||
data->gyro_data.back().data = Eigen::Vector3d(
|
||||
imu_msg->angular_velocity.x, imu_msg->angular_velocity.y,
|
||||
imu_msg->angular_velocity.z);
|
||||
|
||||
min_time = std::min(min_time, time);
|
||||
max_time = std::max(max_time, time);
|
||||
|
||||
int64_t msg_arrival_time = m.getTime().toNSec();
|
||||
system_to_imu_offset_vec.push_back(time - msg_arrival_time);
|
||||
}
|
||||
|
||||
if (mocap_topic == topic) {
|
||||
geometry_msgs::TransformStampedConstPtr mocap_msg =
|
||||
m.instantiate<geometry_msgs::TransformStamped>();
|
||||
|
||||
// Try different message type if instantiate did not work
|
||||
if (!mocap_msg) {
|
||||
geometry_msgs::PoseStampedConstPtr mocap_pose_msg =
|
||||
m.instantiate<geometry_msgs::PoseStamped>();
|
||||
|
||||
geometry_msgs::TransformStampedPtr mocap_new_msg(
|
||||
new geometry_msgs::TransformStamped);
|
||||
mocap_new_msg->header = mocap_pose_msg->header;
|
||||
mocap_new_msg->transform.rotation = mocap_pose_msg->pose.orientation;
|
||||
mocap_new_msg->transform.translation.x =
|
||||
mocap_pose_msg->pose.position.x;
|
||||
mocap_new_msg->transform.translation.y =
|
||||
mocap_pose_msg->pose.position.y;
|
||||
mocap_new_msg->transform.translation.z =
|
||||
mocap_pose_msg->pose.position.z;
|
||||
|
||||
mocap_msg = mocap_new_msg;
|
||||
}
|
||||
|
||||
int64_t time = mocap_msg->header.stamp.toNSec();
|
||||
|
||||
mocap_msgs.push_back(mocap_msg);
|
||||
|
||||
int64_t msg_arrival_time = m.getTime().toNSec();
|
||||
system_to_mocap_offset_vec.push_back(time - msg_arrival_time);
|
||||
}
|
||||
|
||||
if (point_topic == topic) {
|
||||
geometry_msgs::PointStampedConstPtr mocap_msg =
|
||||
m.instantiate<geometry_msgs::PointStamped>();
|
||||
|
||||
int64_t time = mocap_msg->header.stamp.toNSec();
|
||||
|
||||
point_msgs.push_back(mocap_msg);
|
||||
|
||||
int64_t msg_arrival_time = m.getTime().toNSec();
|
||||
system_to_mocap_offset_vec.push_back(time - msg_arrival_time);
|
||||
}
|
||||
|
||||
num_msgs++;
|
||||
}
|
||||
|
||||
data->image_timestamps.clear();
|
||||
data->image_timestamps.insert(data->image_timestamps.begin(),
|
||||
image_timestamps.begin(),
|
||||
image_timestamps.end());
|
||||
|
||||
if (system_to_mocap_offset_vec.size() > 0) {
|
||||
int64_t system_to_imu_offset =
|
||||
system_to_imu_offset_vec[system_to_imu_offset_vec.size() / 2];
|
||||
|
||||
int64_t system_to_mocap_offset =
|
||||
system_to_mocap_offset_vec[system_to_mocap_offset_vec.size() / 2];
|
||||
|
||||
data->mocap_to_imu_offset_ns =
|
||||
system_to_imu_offset - system_to_mocap_offset;
|
||||
}
|
||||
|
||||
data->gt_pose_data.clear();
|
||||
data->gt_timestamps.clear();
|
||||
|
||||
if (!mocap_msgs.empty())
|
||||
for (size_t i = 0; i < mocap_msgs.size() - 1; i++) {
|
||||
auto mocap_msg = mocap_msgs[i];
|
||||
|
||||
int64_t time = mocap_msg->header.stamp.toNSec();
|
||||
|
||||
Eigen::Quaterniond q(
|
||||
mocap_msg->transform.rotation.w, mocap_msg->transform.rotation.x,
|
||||
mocap_msg->transform.rotation.y, mocap_msg->transform.rotation.z);
|
||||
|
||||
Eigen::Vector3d t(mocap_msg->transform.translation.x,
|
||||
mocap_msg->transform.translation.y,
|
||||
mocap_msg->transform.translation.z);
|
||||
|
||||
int64_t timestamp_ns = time + data->mocap_to_imu_offset_ns;
|
||||
data->gt_timestamps.emplace_back(timestamp_ns);
|
||||
data->gt_pose_data.emplace_back(q, t);
|
||||
}
|
||||
|
||||
if (!point_msgs.empty())
|
||||
for (size_t i = 0; i < point_msgs.size() - 1; i++) {
|
||||
auto point_msg = point_msgs[i];
|
||||
|
||||
int64_t time = point_msg->header.stamp.toNSec();
|
||||
|
||||
Eigen::Vector3d t(point_msg->point.x, point_msg->point.y,
|
||||
point_msg->point.z);
|
||||
|
||||
int64_t timestamp_ns = time; // + data->mocap_to_imu_offset_ns;
|
||||
data->gt_timestamps.emplace_back(timestamp_ns);
|
||||
data->gt_pose_data.emplace_back(Sophus::SO3d(), t);
|
||||
}
|
||||
|
||||
std::cout << "Total number of messages: " << num_msgs << std::endl;
|
||||
std::cout << "Image size: " << data->image_data_idx.size() << std::endl;
|
||||
|
||||
std::cout << "Min time: " << min_time << " max time: " << max_time
|
||||
<< " mocap to imu offset: " << data->mocap_to_imu_offset_ns
|
||||
<< std::endl;
|
||||
|
||||
std::cout << "Number of mocap poses: " << data->gt_timestamps.size()
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
void reset() { data.reset(); }
|
||||
|
||||
VioDatasetPtr get_data() {
|
||||
// return std::dynamic_pointer_cast<VioDataset>(data);
|
||||
return data;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<RosbagVioDataset> data;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
#endif // DATASET_IO_ROSBAG_H
|
||||
316
include/basalt/io/dataset_io_uzh.h
Normal file
316
include/basalt/io/dataset_io_uzh.h
Normal file
@@ -0,0 +1,316 @@
|
||||
/**
|
||||
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/io/dataset_io.h>
|
||||
#include <basalt/utils/filesystem.h>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class UzhVioDataset : public VioDataset {
|
||||
size_t num_cams;
|
||||
|
||||
std::string path;
|
||||
|
||||
std::vector<int64_t> image_timestamps;
|
||||
std::unordered_map<int64_t, std::string> left_image_path;
|
||||
std::unordered_map<int64_t, std::string> right_image_path;
|
||||
|
||||
// vector of images for every timestamp
|
||||
// assumes vectors size is num_cams for every timestamp with null pointers for
|
||||
// missing frames
|
||||
// std::unordered_map<int64_t, std::vector<ImageData>> image_data;
|
||||
|
||||
Eigen::aligned_vector<AccelData> accel_data;
|
||||
Eigen::aligned_vector<GyroData> gyro_data;
|
||||
|
||||
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
|
||||
Eigen::aligned_vector<Sophus::SE3d>
|
||||
gt_pose_data; // TODO: change to eigen aligned
|
||||
|
||||
int64_t mocap_to_imu_offset_ns = 0;
|
||||
|
||||
std::vector<std::unordered_map<int64_t, double>> exposure_times;
|
||||
|
||||
public:
|
||||
~UzhVioDataset(){};
|
||||
|
||||
size_t get_num_cams() const { return num_cams; }
|
||||
|
||||
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
|
||||
|
||||
const Eigen::aligned_vector<AccelData> &get_accel_data() const {
|
||||
return accel_data;
|
||||
}
|
||||
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
|
||||
return gyro_data;
|
||||
}
|
||||
const std::vector<int64_t> &get_gt_timestamps() const {
|
||||
return gt_timestamps;
|
||||
}
|
||||
const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
|
||||
return gt_pose_data;
|
||||
}
|
||||
|
||||
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
|
||||
|
||||
std::vector<ImageData> get_image_data(int64_t t_ns) {
|
||||
std::vector<ImageData> res(num_cams);
|
||||
|
||||
for (size_t i = 0; i < num_cams; i++) {
|
||||
std::string full_image_path =
|
||||
path + "/" +
|
||||
(i == 0 ? left_image_path.at(t_ns) : right_image_path.at(t_ns));
|
||||
|
||||
if (fs::exists(full_image_path)) {
|
||||
cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
|
||||
|
||||
if (img.type() == CV_8UC1) {
|
||||
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
|
||||
|
||||
const uint8_t *data_in = img.ptr();
|
||||
uint16_t *data_out = res[i].img->ptr;
|
||||
|
||||
size_t full_size = img.cols * img.rows;
|
||||
for (size_t i = 0; i < full_size; i++) {
|
||||
int val = data_in[i];
|
||||
val = val << 8;
|
||||
data_out[i] = val;
|
||||
}
|
||||
} else if (img.type() == CV_8UC3) {
|
||||
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
|
||||
|
||||
const uint8_t *data_in = img.ptr();
|
||||
uint16_t *data_out = res[i].img->ptr;
|
||||
|
||||
size_t full_size = img.cols * img.rows;
|
||||
for (size_t i = 0; i < full_size; i++) {
|
||||
int val = data_in[i * 3];
|
||||
val = val << 8;
|
||||
data_out[i] = val;
|
||||
}
|
||||
} else if (img.type() == CV_16UC1) {
|
||||
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
|
||||
std::memcpy(res[i].img->ptr, img.ptr(),
|
||||
img.cols * img.rows * sizeof(uint16_t));
|
||||
|
||||
} else {
|
||||
std::cerr << "img.fmt.bpp " << img.type() << std::endl;
|
||||
std::abort();
|
||||
}
|
||||
|
||||
auto exp_it = exposure_times[i].find(t_ns);
|
||||
if (exp_it != exposure_times[i].end()) {
|
||||
res[i].exposure = exp_it->second;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
|
||||
cv::Mat mat = cv::Mat(1, 10, CV_32F, 1);
|
||||
return mat;
|
||||
}
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
friend class UzhIO;
|
||||
};
|
||||
|
||||
class UzhIO : public DatasetIoInterface {
|
||||
public:
|
||||
UzhIO() {}
|
||||
|
||||
void read(const std::string &path) {
|
||||
if (!fs::exists(path))
|
||||
std::cerr << "No dataset found in " << path << std::endl;
|
||||
|
||||
data.reset(new UzhVioDataset);
|
||||
|
||||
data->num_cams = 2;
|
||||
data->path = path;
|
||||
|
||||
read_image_timestamps(path);
|
||||
|
||||
std::cout << "Loaded " << data->get_image_timestamps().size()
|
||||
<< " timestamps, " << data->left_image_path.size()
|
||||
<< " left images and " << data->right_image_path.size()
|
||||
<< std::endl;
|
||||
|
||||
// {
|
||||
// int64_t t_ns = data->get_image_timestamps()[0];
|
||||
// std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " "
|
||||
// << data->right_image_path.at(t_ns) << std::endl;
|
||||
// }
|
||||
|
||||
read_imu_data(path + "/imu.txt");
|
||||
|
||||
std::cout << "Loaded " << data->get_gyro_data().size() << " imu msgs."
|
||||
<< std::endl;
|
||||
|
||||
if (fs::exists(path + "/groundtruth.txt")) {
|
||||
read_gt_data_pose(path + "/groundtruth.txt");
|
||||
}
|
||||
|
||||
data->exposure_times.resize(data->num_cams);
|
||||
}
|
||||
|
||||
void reset() { data.reset(); }
|
||||
|
||||
VioDatasetPtr get_data() { return data; }
|
||||
|
||||
private:
|
||||
void read_exposure(const std::string &path,
|
||||
std::unordered_map<int64_t, double> &exposure_data) {
|
||||
exposure_data.clear();
|
||||
|
||||
std::ifstream f(path + "exposure.csv");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
char tmp;
|
||||
int64_t timestamp, exposure_int;
|
||||
Eigen::Vector3d gyro, accel;
|
||||
|
||||
ss >> timestamp >> tmp >> exposure_int;
|
||||
|
||||
exposure_data[timestamp] = exposure_int * 1e-9;
|
||||
}
|
||||
}
|
||||
|
||||
void read_image_timestamps(const std::string &path) {
|
||||
{
|
||||
std::ifstream f(path + "/left_images.txt");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
std::stringstream ss(line);
|
||||
int tmp;
|
||||
double t_s;
|
||||
std::string path;
|
||||
ss >> tmp >> t_s >> path;
|
||||
|
||||
int64_t t_ns = t_s * 1e9;
|
||||
|
||||
data->image_timestamps.emplace_back(t_ns);
|
||||
data->left_image_path[t_ns] = path;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
std::ifstream f(path + "/right_images.txt");
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
std::stringstream ss(line);
|
||||
int tmp;
|
||||
double t_s;
|
||||
std::string path;
|
||||
ss >> tmp >> t_s >> path;
|
||||
|
||||
int64_t t_ns = t_s * 1e9;
|
||||
|
||||
data->right_image_path[t_ns] = path;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void read_imu_data(const std::string &path) {
|
||||
data->accel_data.clear();
|
||||
data->gyro_data.clear();
|
||||
|
||||
std::ifstream f(path);
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
int tmp;
|
||||
double timestamp;
|
||||
Eigen::Vector3d gyro, accel;
|
||||
|
||||
ss >> tmp >> timestamp >> gyro[0] >> gyro[1] >> gyro[2] >> accel[0] >>
|
||||
accel[1] >> accel[2];
|
||||
|
||||
int64_t t_ns = timestamp * 1e9;
|
||||
|
||||
data->accel_data.emplace_back();
|
||||
data->accel_data.back().timestamp_ns = t_ns;
|
||||
data->accel_data.back().data = accel;
|
||||
|
||||
data->gyro_data.emplace_back();
|
||||
data->gyro_data.back().timestamp_ns = t_ns;
|
||||
data->gyro_data.back().data = gyro;
|
||||
}
|
||||
}
|
||||
|
||||
void read_gt_data_pose(const std::string &path) {
|
||||
data->gt_timestamps.clear();
|
||||
data->gt_pose_data.clear();
|
||||
|
||||
std::ifstream f(path);
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '#') continue;
|
||||
|
||||
std::stringstream ss(line);
|
||||
|
||||
int tmp;
|
||||
double timestamp;
|
||||
Eigen::Quaterniond q;
|
||||
Eigen::Vector3d pos;
|
||||
|
||||
ss >> tmp >> timestamp >> pos[0] >> pos[1] >> pos[2] >> q.x() >> q.y() >>
|
||||
q.z() >> q.w();
|
||||
|
||||
int64_t t_ns = timestamp * 1e9;
|
||||
|
||||
data->gt_timestamps.emplace_back(t_ns);
|
||||
data->gt_pose_data.emplace_back(q, pos);
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<UzhVioDataset> data;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
78
include/basalt/io/marg_data_io.h
Normal file
78
include/basalt/io/marg_data_io.h
Normal file
@@ -0,0 +1,78 @@
|
||||
/**
|
||||
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 <basalt/utils/imu_types.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class MargDataSaver {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<MargDataSaver>;
|
||||
|
||||
MargDataSaver(const std::string& path);
|
||||
~MargDataSaver() {
|
||||
saving_thread->join();
|
||||
saving_img_thread->join();
|
||||
}
|
||||
tbb::concurrent_bounded_queue<MargData::Ptr> in_marg_queue;
|
||||
|
||||
private:
|
||||
std::shared_ptr<std::thread> saving_thread;
|
||||
std::shared_ptr<std::thread> saving_img_thread;
|
||||
|
||||
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr> save_image_queue;
|
||||
};
|
||||
|
||||
class MargDataLoader {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<MargDataLoader>;
|
||||
|
||||
MargDataLoader();
|
||||
|
||||
void start(const std::string& path);
|
||||
~MargDataLoader() {
|
||||
if (processing_thread) processing_thread->join();
|
||||
}
|
||||
|
||||
tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue;
|
||||
|
||||
private:
|
||||
std::shared_ptr<std::thread> processing_thread;
|
||||
};
|
||||
} // namespace basalt
|
||||
89
include/basalt/linearization/block_diagonal.hpp
Normal file
89
include/basalt/linearization/block_diagonal.hpp
Normal file
@@ -0,0 +1,89 @@
|
||||
#pragma once
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <basalt/utils/cast_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
// TODO: expand IndexedBlocks to small class / struct that also holds info on
|
||||
// block size and number of blocks, so we don't have to pass it around all the
|
||||
// time and we can directly implement things link adding diagonal and matrix
|
||||
// vector products in this sparse block diagonal matrix.
|
||||
|
||||
// map of camera index to block; used to represent sparse block diagonal matrix
|
||||
template <typename Scalar>
|
||||
using IndexedBlocks =
|
||||
std::unordered_map<size_t,
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>;
|
||||
|
||||
// scale dimensions of JTJ as you would do for jacobian scaling of J beforehand,
|
||||
// with diagonal scaling matrix D: For jacobain we would use JD, so for JTJ we
|
||||
// use DJTJD.
|
||||
template <typename Scalar>
|
||||
void scale_jacobians(
|
||||
IndexedBlocks<Scalar>& block_diagonal, size_t num_blocks, size_t block_size,
|
||||
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& scaling_vector) {
|
||||
BASALT_ASSERT(num_blocks * block_size ==
|
||||
unsigned_cast(scaling_vector.size()));
|
||||
for (auto& [cam_idx, block] : block_diagonal) {
|
||||
auto D =
|
||||
scaling_vector.segment(block_size * cam_idx, block_size).asDiagonal();
|
||||
block = D * block * D;
|
||||
}
|
||||
}
|
||||
|
||||
// add diagonal to block-diagonal matrix; missing blocks are assumed to be 0
|
||||
template <typename Scalar>
|
||||
void add_diagonal(IndexedBlocks<Scalar>& block_diagonal, size_t num_blocks,
|
||||
size_t block_size,
|
||||
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& diagonal) {
|
||||
BASALT_ASSERT(num_blocks * block_size == unsigned_cast(diagonal.size()));
|
||||
for (size_t idx = 0; idx < num_blocks; ++idx) {
|
||||
auto [it, is_new] = block_diagonal.try_emplace(idx);
|
||||
if (is_new) {
|
||||
it->second = diagonal.segment(block_size * idx, block_size).asDiagonal();
|
||||
} else {
|
||||
it->second += diagonal.segment(block_size * idx, block_size).asDiagonal();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// sum up diagonal blocks in hash map for parallel reduction
|
||||
template <typename Scalar>
|
||||
class BlockDiagonalAccumulator {
|
||||
public:
|
||||
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
inline void add(size_t idx, MatX&& block) {
|
||||
auto [it, is_new] = block_diagonal_.try_emplace(idx);
|
||||
if (is_new) {
|
||||
it->second = std::move(block);
|
||||
} else {
|
||||
it->second += block;
|
||||
}
|
||||
}
|
||||
|
||||
inline void add_diag(size_t num_blocks, size_t block_size,
|
||||
const VecX& diagonal) {
|
||||
add_diagonal(block_diagonal_, num_blocks, block_size, diagonal);
|
||||
}
|
||||
|
||||
inline void join(BlockDiagonalAccumulator& b) {
|
||||
for (auto& [k, v] : b.block_diagonal_) {
|
||||
auto [it, is_new] = block_diagonal_.try_emplace(k);
|
||||
if (is_new) {
|
||||
it->second = std::move(v);
|
||||
} else {
|
||||
it->second += v;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
IndexedBlocks<Scalar> block_diagonal_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
235
include/basalt/linearization/imu_block.hpp
Normal file
235
include/basalt/linearization/imu_block.hpp
Normal file
@@ -0,0 +1,235 @@
|
||||
#pragma once
|
||||
|
||||
#include <basalt/imu/preintegration.h>
|
||||
#include <basalt/optimization/accumulator.h>
|
||||
#include <basalt/utils/imu_types.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Scalar_>
|
||||
class ImuBlock {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
|
||||
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
ImuBlock(const IntegratedImuMeasurement<Scalar>* meas,
|
||||
const ImuLinData<Scalar>* imu_lin_data, const AbsOrderMap& aom)
|
||||
: imu_meas(meas), imu_lin_data(imu_lin_data), aom(aom) {
|
||||
Jp.resize(POSE_VEL_BIAS_SIZE, 2 * POSE_VEL_BIAS_SIZE);
|
||||
r.resize(POSE_VEL_BIAS_SIZE);
|
||||
}
|
||||
|
||||
Scalar linearizeImu(
|
||||
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
|
||||
frame_states) {
|
||||
Jp.setZero();
|
||||
r.setZero();
|
||||
|
||||
const int64_t start_t = imu_meas->get_start_t_ns();
|
||||
const int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
|
||||
|
||||
const size_t start_idx = 0;
|
||||
const size_t end_idx = POSE_VEL_BIAS_SIZE;
|
||||
|
||||
PoseVelBiasStateWithLin<Scalar> start_state = frame_states.at(start_t);
|
||||
PoseVelBiasStateWithLin<Scalar> end_state = frame_states.at(end_t);
|
||||
|
||||
typename IntegratedImuMeasurement<Scalar>::MatNN d_res_d_start, d_res_d_end;
|
||||
typename IntegratedImuMeasurement<Scalar>::MatN3 d_res_d_bg, d_res_d_ba;
|
||||
|
||||
typename PoseVelState<Scalar>::VecN res = imu_meas->residual(
|
||||
start_state.getStateLin(), imu_lin_data->g, end_state.getStateLin(),
|
||||
start_state.getStateLin().bias_gyro,
|
||||
start_state.getStateLin().bias_accel, &d_res_d_start, &d_res_d_end,
|
||||
&d_res_d_bg, &d_res_d_ba);
|
||||
|
||||
if (start_state.isLinearized() || end_state.isLinearized()) {
|
||||
res = imu_meas->residual(
|
||||
start_state.getState(), imu_lin_data->g, end_state.getState(),
|
||||
start_state.getState().bias_gyro, start_state.getState().bias_accel);
|
||||
}
|
||||
|
||||
// error
|
||||
Scalar imu_error =
|
||||
Scalar(0.5) * (imu_meas->get_sqrt_cov_inv() * res).squaredNorm();
|
||||
|
||||
// imu residual linearization
|
||||
Jp.template block<9, 9>(0, start_idx) =
|
||||
imu_meas->get_sqrt_cov_inv() * d_res_d_start;
|
||||
Jp.template block<9, 9>(0, end_idx) =
|
||||
imu_meas->get_sqrt_cov_inv() * d_res_d_end;
|
||||
|
||||
Jp.template block<9, 3>(0, start_idx + 9) =
|
||||
imu_meas->get_sqrt_cov_inv() * d_res_d_bg;
|
||||
Jp.template block<9, 3>(0, start_idx + 12) =
|
||||
imu_meas->get_sqrt_cov_inv() * d_res_d_ba;
|
||||
|
||||
r.template segment<9>(0) = imu_meas->get_sqrt_cov_inv() * res;
|
||||
|
||||
// difference between biases
|
||||
Scalar dt = imu_meas->get_dt_ns() * Scalar(1e-9);
|
||||
|
||||
Vec3 gyro_bias_weight_dt =
|
||||
imu_lin_data->gyro_bias_weight_sqrt / std::sqrt(dt);
|
||||
Vec3 res_bg =
|
||||
start_state.getState().bias_gyro - end_state.getState().bias_gyro;
|
||||
|
||||
Jp.template block<3, 3>(9, start_idx + 9) =
|
||||
gyro_bias_weight_dt.asDiagonal();
|
||||
Jp.template block<3, 3>(9, end_idx + 9) =
|
||||
(-gyro_bias_weight_dt).asDiagonal();
|
||||
|
||||
r.template segment<3>(9) += gyro_bias_weight_dt.asDiagonal() * res_bg;
|
||||
|
||||
Scalar bg_error =
|
||||
Scalar(0.5) * (gyro_bias_weight_dt.asDiagonal() * res_bg).squaredNorm();
|
||||
|
||||
Vec3 accel_bias_weight_dt =
|
||||
imu_lin_data->accel_bias_weight_sqrt / std::sqrt(dt);
|
||||
Vec3 res_ba =
|
||||
start_state.getState().bias_accel - end_state.getState().bias_accel;
|
||||
|
||||
Jp.template block<3, 3>(12, start_idx + 12) =
|
||||
accel_bias_weight_dt.asDiagonal();
|
||||
Jp.template block<3, 3>(12, end_idx + 12) =
|
||||
(-accel_bias_weight_dt).asDiagonal();
|
||||
|
||||
r.template segment<3>(12) += accel_bias_weight_dt.asDiagonal() * res_ba;
|
||||
|
||||
Scalar ba_error =
|
||||
Scalar(0.5) *
|
||||
(accel_bias_weight_dt.asDiagonal() * res_ba).squaredNorm();
|
||||
|
||||
return imu_error + bg_error + ba_error;
|
||||
}
|
||||
|
||||
void add_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r, size_t row_start_idx) const {
|
||||
int64_t start_t = imu_meas->get_start_t_ns();
|
||||
int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
|
||||
|
||||
const size_t start_idx = aom.abs_order_map.at(start_t).first;
|
||||
const size_t end_idx = aom.abs_order_map.at(end_t).first;
|
||||
|
||||
Q2Jp.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(row_start_idx,
|
||||
start_idx) +=
|
||||
Jp.template topLeftCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>();
|
||||
|
||||
Q2Jp.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(row_start_idx,
|
||||
end_idx) +=
|
||||
Jp.template topRightCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>();
|
||||
|
||||
Q2r.template segment<POSE_VEL_BIAS_SIZE>(row_start_idx) += r;
|
||||
}
|
||||
|
||||
void add_dense_H_b(DenseAccumulator<Scalar>& accum) const {
|
||||
int64_t start_t = imu_meas->get_start_t_ns();
|
||||
int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
|
||||
|
||||
const size_t start_idx = aom.abs_order_map.at(start_t).first;
|
||||
const size_t end_idx = aom.abs_order_map.at(end_t).first;
|
||||
|
||||
const MatX H = Jp.transpose() * Jp;
|
||||
const VecX b = Jp.transpose() * r;
|
||||
|
||||
accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
|
||||
start_idx, start_idx,
|
||||
H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(0, 0));
|
||||
|
||||
accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
|
||||
end_idx, start_idx,
|
||||
H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
|
||||
POSE_VEL_BIAS_SIZE, 0));
|
||||
|
||||
accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
|
||||
start_idx, end_idx,
|
||||
H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
|
||||
0, POSE_VEL_BIAS_SIZE));
|
||||
|
||||
accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
|
||||
end_idx, end_idx,
|
||||
H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
|
||||
POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE));
|
||||
|
||||
accum.template addB<POSE_VEL_BIAS_SIZE>(
|
||||
start_idx, b.template segment<POSE_VEL_BIAS_SIZE>(0));
|
||||
accum.template addB<POSE_VEL_BIAS_SIZE>(
|
||||
end_idx, b.template segment<POSE_VEL_BIAS_SIZE>(POSE_VEL_BIAS_SIZE));
|
||||
}
|
||||
|
||||
void scaleJp_cols(const VecX& jacobian_scaling) {
|
||||
int64_t start_t = imu_meas->get_start_t_ns();
|
||||
int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
|
||||
|
||||
const size_t start_idx = aom.abs_order_map.at(start_t).first;
|
||||
const size_t end_idx = aom.abs_order_map.at(end_t).first;
|
||||
|
||||
Jp.template topLeftCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>() *=
|
||||
jacobian_scaling.template segment<POSE_VEL_BIAS_SIZE>(start_idx)
|
||||
.asDiagonal();
|
||||
|
||||
Jp.template topRightCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>() *=
|
||||
jacobian_scaling.template segment<POSE_VEL_BIAS_SIZE>(end_idx)
|
||||
.asDiagonal();
|
||||
}
|
||||
|
||||
void addJp_diag2(VecX& res) const {
|
||||
int64_t start_t = imu_meas->get_start_t_ns();
|
||||
int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
|
||||
|
||||
const size_t start_idx = aom.abs_order_map.at(start_t).first;
|
||||
const size_t end_idx = aom.abs_order_map.at(end_t).first;
|
||||
|
||||
res.template segment<POSE_VEL_BIAS_SIZE>(start_idx) +=
|
||||
Jp.template topLeftCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>()
|
||||
.colwise()
|
||||
.squaredNorm();
|
||||
|
||||
res.template segment<POSE_VEL_BIAS_SIZE>(end_idx) +=
|
||||
Jp.template topRightCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>()
|
||||
.colwise()
|
||||
.squaredNorm();
|
||||
}
|
||||
|
||||
void backSubstitute(const VecX& pose_inc, Scalar& l_diff) {
|
||||
int64_t start_t = imu_meas->get_start_t_ns();
|
||||
int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
|
||||
|
||||
const size_t start_idx = aom.abs_order_map.at(start_t).first;
|
||||
const size_t end_idx = aom.abs_order_map.at(end_t).first;
|
||||
|
||||
VecX pose_inc_reduced(2 * POSE_VEL_BIAS_SIZE);
|
||||
pose_inc_reduced.template head<POSE_VEL_BIAS_SIZE>() =
|
||||
pose_inc.template segment<POSE_VEL_BIAS_SIZE>(start_idx);
|
||||
pose_inc_reduced.template tail<POSE_VEL_BIAS_SIZE>() =
|
||||
pose_inc.template segment<POSE_VEL_BIAS_SIZE>(end_idx);
|
||||
|
||||
// We want to compute the model cost change. The model function is
|
||||
//
|
||||
// L(inc) = F(x) + incT JT r + 0.5 incT JT J inc
|
||||
//
|
||||
// and thus the expect decrease in cost for the computed increment is
|
||||
//
|
||||
// l_diff = L(0) - L(inc)
|
||||
// = - incT JT r - 0.5 incT JT J inc.
|
||||
// = - incT JT (r + 0.5 J inc)
|
||||
// = - (J inc)T (r + 0.5 (J inc))
|
||||
|
||||
VecX Jinc = Jp * pose_inc_reduced;
|
||||
l_diff -= Jinc.transpose() * (Scalar(0.5) * Jinc + r);
|
||||
}
|
||||
|
||||
protected:
|
||||
std::array<FrameId, 2> frame_ids;
|
||||
MatX Jp;
|
||||
VecX r;
|
||||
|
||||
const IntegratedImuMeasurement<Scalar>* imu_meas;
|
||||
const ImuLinData<Scalar>* imu_lin_data;
|
||||
const AbsOrderMap& aom;
|
||||
}; // namespace basalt
|
||||
|
||||
} // namespace basalt
|
||||
140
include/basalt/linearization/landmark_block.hpp
Normal file
140
include/basalt/linearization/landmark_block.hpp
Normal file
@@ -0,0 +1,140 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Sparse>
|
||||
|
||||
#include <basalt/optimization/accumulator.h>
|
||||
#include <basalt/vi_estimator/landmark_database.h>
|
||||
#include <basalt/linearization/block_diagonal.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Scalar>
|
||||
struct RelPoseLin {
|
||||
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
|
||||
using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
|
||||
|
||||
Mat4 T_t_h;
|
||||
Mat6 d_rel_d_h;
|
||||
Mat6 d_rel_d_t;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
template <typename Scalar>
|
||||
class LandmarkBlock {
|
||||
public:
|
||||
struct Options {
|
||||
// use Householder instead of Givens for marginalization
|
||||
bool use_householder = true;
|
||||
|
||||
// use_valid_projections_only: if true, set invalid projection's
|
||||
// residual and jacobian to 0; invalid means z <= 0
|
||||
bool use_valid_projections_only = true;
|
||||
|
||||
// if > 0, use huber norm with given threshold, else squared norm
|
||||
Scalar huber_parameter = 0;
|
||||
|
||||
// Standard deviation of reprojection error to weight visual measurements
|
||||
Scalar obs_std_dev = 1;
|
||||
|
||||
// ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
|
||||
// we use 1.0 / (eps + sqrt(SquaredColumnNorm))
|
||||
Scalar jacobi_scaling_eps = 1e-6;
|
||||
};
|
||||
|
||||
enum State {
|
||||
Uninitialized = 0,
|
||||
Allocated,
|
||||
NumericalFailure,
|
||||
Linearized,
|
||||
Marginalized
|
||||
};
|
||||
|
||||
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 Mat36 = Eigen::Matrix<Scalar, 3, 6>;
|
||||
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
using RowMatX =
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
|
||||
|
||||
using RowMat3 = Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>;
|
||||
|
||||
virtual ~LandmarkBlock(){};
|
||||
|
||||
virtual bool isNumericalFailure() const = 0;
|
||||
virtual void allocateLandmark(
|
||||
Keypoint<Scalar>& lm,
|
||||
const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
|
||||
RelPoseLin<Scalar>>& relative_pose_lin,
|
||||
const Calibration<Scalar>& calib, const AbsOrderMap& aom,
|
||||
const Options& options,
|
||||
const std::map<TimeCamId, size_t>* rel_order = nullptr) = 0;
|
||||
|
||||
// may set state to NumericalFailure --> linearization at this state is
|
||||
// unusable. Numeric check is only performed for residuals that were
|
||||
// considered to be used (valid), which depends on
|
||||
// use_valid_projections_only setting.
|
||||
virtual Scalar linearizeLandmark() = 0;
|
||||
virtual void performQR() = 0;
|
||||
|
||||
// Sets damping and maintains upper triangular matrix for landmarks.
|
||||
virtual void setLandmarkDamping(Scalar lambda) = 0;
|
||||
|
||||
// lambda < 0 means computing exact model cost change
|
||||
virtual void backSubstitute(const VecX& pose_inc, Scalar& l_diff) = 0;
|
||||
|
||||
virtual void addQ2JpTQ2Jp_mult_x(VecX& res, const VecX& x_pose) const = 0;
|
||||
|
||||
virtual void addQ2JpTQ2r(VecX& res) const = 0;
|
||||
|
||||
virtual void addJp_diag2(VecX& res) const = 0;
|
||||
|
||||
virtual void addQ2JpTQ2Jp_blockdiag(
|
||||
BlockDiagonalAccumulator<Scalar>& accu) const = 0;
|
||||
|
||||
virtual void scaleJl_cols() = 0;
|
||||
virtual void scaleJp_cols(const VecX& jacobian_scaling) = 0;
|
||||
virtual void printStorage(const std::string& filename) const = 0;
|
||||
virtual State getState() const = 0;
|
||||
|
||||
virtual size_t numReducedCams() const = 0;
|
||||
|
||||
virtual void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
|
||||
size_t start_idx) const = 0;
|
||||
|
||||
virtual void get_dense_Q2Jp_Q2r_rel(
|
||||
MatX& Q2Jp, VecX& Q2r, size_t start_idx,
|
||||
const std::map<TimeCamId, size_t>& rel_order) const = 0;
|
||||
|
||||
virtual void add_dense_H_b(DenseAccumulator<Scalar>& accum) const = 0;
|
||||
|
||||
virtual void add_dense_H_b(MatX& H, VecX& b) const = 0;
|
||||
|
||||
virtual void add_dense_H_b_rel(
|
||||
MatX& H_rel, VecX& b_rel,
|
||||
const std::map<TimeCamId, size_t>& rel_order) const = 0;
|
||||
|
||||
virtual const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
|
||||
const = 0;
|
||||
|
||||
virtual Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
|
||||
const std::map<TimeCamId, size_t>& rel_order) const = 0;
|
||||
|
||||
virtual void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const = 0;
|
||||
|
||||
virtual TimeCamId getHostKf() const = 0;
|
||||
|
||||
virtual size_t numQ2rows() const = 0;
|
||||
|
||||
// factory method
|
||||
template <int POSE_SIZE>
|
||||
static std::unique_ptr<LandmarkBlock<Scalar>> createLandmarkBlock();
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
585
include/basalt/linearization/landmark_block_abs_dynamic.hpp
Normal file
585
include/basalt/linearization/landmark_block_abs_dynamic.hpp
Normal file
@@ -0,0 +1,585 @@
|
||||
#pragma once
|
||||
|
||||
#include <fstream>
|
||||
#include <mutex>
|
||||
|
||||
#include <basalt/utils/ba_utils.h>
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Sparse>
|
||||
#include <basalt/linearization/landmark_block.hpp>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <typename Scalar, int POSE_SIZE>
|
||||
class LandmarkBlockAbsDynamic : public LandmarkBlock<Scalar> {
|
||||
public:
|
||||
using Options = typename LandmarkBlock<Scalar>::Options;
|
||||
using State = typename LandmarkBlock<Scalar>::State;
|
||||
|
||||
inline bool isNumericalFailure() const override {
|
||||
return state == State::NumericalFailure;
|
||||
}
|
||||
|
||||
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 Mat36 = Eigen::Matrix<Scalar, 3, 6>;
|
||||
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
using RowMatX =
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
|
||||
|
||||
virtual inline void allocateLandmark(
|
||||
Keypoint<Scalar>& lm,
|
||||
const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
|
||||
RelPoseLin<Scalar>>& relative_pose_lin,
|
||||
const Calibration<Scalar>& calib, const AbsOrderMap& aom,
|
||||
const Options& options,
|
||||
const std::map<TimeCamId, size_t>* rel_order = nullptr) override {
|
||||
// some of the logic assumes the members are at their initial values
|
||||
BASALT_ASSERT(state == State::Uninitialized);
|
||||
|
||||
UNUSED(rel_order);
|
||||
|
||||
lm_ptr = &lm;
|
||||
options_ = &options;
|
||||
calib_ = &calib;
|
||||
|
||||
// TODO: consider for VIO that we have a lot of 0 columns if we just use aom
|
||||
// --> add option to use different AOM with reduced size and/or just
|
||||
// involved poses --> when accumulating results, check which case we have;
|
||||
// if both aom are identical, we don't have to do block-wise operations.
|
||||
aom_ = &aom;
|
||||
|
||||
pose_lin_vec.clear();
|
||||
pose_lin_vec.reserve(lm.obs.size());
|
||||
pose_tcid_vec.clear();
|
||||
pose_tcid_vec.reserve(lm.obs.size());
|
||||
|
||||
// LMBs without host frame should not be created
|
||||
BASALT_ASSERT(aom.abs_order_map.count(lm.host_kf_id.frame_id) > 0);
|
||||
|
||||
for (const auto& [tcid_t, pos] : lm.obs) {
|
||||
size_t i = pose_lin_vec.size();
|
||||
|
||||
auto it = relative_pose_lin.find(std::make_pair(lm.host_kf_id, tcid_t));
|
||||
BASALT_ASSERT(it != relative_pose_lin.end());
|
||||
|
||||
if (aom.abs_order_map.count(tcid_t.frame_id) > 0) {
|
||||
pose_lin_vec.push_back(&it->second);
|
||||
} else {
|
||||
// Observation droped for marginalization
|
||||
pose_lin_vec.push_back(nullptr);
|
||||
}
|
||||
pose_tcid_vec.push_back(&it->first);
|
||||
|
||||
res_idx_by_abs_pose_[it->first.first.frame_id].insert(i); // host
|
||||
res_idx_by_abs_pose_[it->first.second.frame_id].insert(i); // target
|
||||
}
|
||||
|
||||
// number of pose-jacobian columns is determined by oam
|
||||
padding_idx = aom_->total_size;
|
||||
|
||||
num_rows = pose_lin_vec.size() * 2 + 3; // residuals and lm damping
|
||||
|
||||
size_t pad = padding_idx % 4;
|
||||
if (pad != 0) {
|
||||
padding_size = 4 - pad;
|
||||
}
|
||||
|
||||
lm_idx = padding_idx + padding_size;
|
||||
res_idx = lm_idx + 3;
|
||||
num_cols = res_idx + 1;
|
||||
|
||||
// number of columns should now be multiple of 4 for good memory alignment
|
||||
// TODO: test extending this to 8 --> 32byte alignment for float?
|
||||
BASALT_ASSERT(num_cols % 4 == 0);
|
||||
|
||||
storage.resize(num_rows, num_cols);
|
||||
|
||||
damping_rotations.clear();
|
||||
damping_rotations.reserve(6);
|
||||
|
||||
state = State::Allocated;
|
||||
}
|
||||
|
||||
// may set state to NumericalFailure --> linearization at this state is
|
||||
// unusable. Numeric check is only performed for residuals that were
|
||||
// considered to be used (valid), which depends on
|
||||
// use_valid_projections_only setting.
|
||||
virtual inline Scalar linearizeLandmark() override {
|
||||
BASALT_ASSERT(state == State::Allocated ||
|
||||
state == State::NumericalFailure ||
|
||||
state == State::Linearized || state == State::Marginalized);
|
||||
|
||||
// storage.setZero(num_rows, num_cols);
|
||||
storage.setZero();
|
||||
damping_rotations.clear();
|
||||
damping_rotations.reserve(6);
|
||||
|
||||
bool numerically_valid = true;
|
||||
|
||||
Scalar error_sum = 0;
|
||||
|
||||
size_t i = 0;
|
||||
for (const auto& [tcid_t, obs] : lm_ptr->obs) {
|
||||
std::visit(
|
||||
[&, obs = obs](const auto& cam) {
|
||||
// TODO: The pose_lin_vec[i] == nullptr is intended to deal with
|
||||
// dropped measurements during marginalization. However, dropped
|
||||
// measurements should only occur for the remaining frames, not for
|
||||
// the marginalized frames. Maybe these are observations bewtween
|
||||
// two marginalized frames, if more than one is marginalized at the
|
||||
// same time? But those we would not have to drop... Double check if
|
||||
// and when this happens and possibly resolve by fixing handling
|
||||
// here, or else updating the measurements in lmdb before calling
|
||||
// linearization. Otherwise, check where else we need a `if
|
||||
// (pose_lin_vec[i])` check or `pose_lin_vec[i] != nullptr` assert
|
||||
// in this class.
|
||||
|
||||
if (pose_lin_vec[i]) {
|
||||
size_t obs_idx = i * 2;
|
||||
size_t abs_h_idx =
|
||||
aom_->abs_order_map.at(pose_tcid_vec[i]->first.frame_id)
|
||||
.first;
|
||||
size_t abs_t_idx =
|
||||
aom_->abs_order_map.at(pose_tcid_vec[i]->second.frame_id)
|
||||
.first;
|
||||
|
||||
Vec2 res;
|
||||
Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
|
||||
Eigen::Matrix<Scalar, 2, 3> d_res_d_p;
|
||||
|
||||
using CamT = std::decay_t<decltype(cam)>;
|
||||
bool valid = linearizePoint<Scalar, CamT>(
|
||||
obs, *lm_ptr, pose_lin_vec[i]->T_t_h, cam, res, &d_res_d_xi,
|
||||
&d_res_d_p);
|
||||
|
||||
if (!options_->use_valid_projections_only || valid) {
|
||||
numerically_valid = numerically_valid &&
|
||||
d_res_d_xi.array().isFinite().all() &&
|
||||
d_res_d_p.array().isFinite().all();
|
||||
|
||||
const Scalar res_squared = res.squaredNorm();
|
||||
const auto [weighted_error, weight] =
|
||||
compute_error_weight(res_squared);
|
||||
const Scalar sqrt_weight =
|
||||
std::sqrt(weight) / options_->obs_std_dev;
|
||||
|
||||
error_sum += weighted_error /
|
||||
(options_->obs_std_dev * options_->obs_std_dev);
|
||||
|
||||
storage.template block<2, 3>(obs_idx, lm_idx) =
|
||||
sqrt_weight * d_res_d_p;
|
||||
storage.template block<2, 1>(obs_idx, res_idx) =
|
||||
sqrt_weight * res;
|
||||
|
||||
d_res_d_xi *= sqrt_weight;
|
||||
storage.template block<2, 6>(obs_idx, abs_h_idx) +=
|
||||
d_res_d_xi * pose_lin_vec[i]->d_rel_d_h;
|
||||
storage.template block<2, 6>(obs_idx, abs_t_idx) +=
|
||||
d_res_d_xi * pose_lin_vec[i]->d_rel_d_t;
|
||||
}
|
||||
}
|
||||
|
||||
i++;
|
||||
},
|
||||
calib_->intrinsics[tcid_t.cam_id].variant);
|
||||
}
|
||||
|
||||
if (numerically_valid) {
|
||||
state = State::Linearized;
|
||||
} else {
|
||||
state = State::NumericalFailure;
|
||||
}
|
||||
|
||||
return error_sum;
|
||||
}
|
||||
|
||||
virtual inline void performQR() override {
|
||||
BASALT_ASSERT(state == State::Linearized);
|
||||
|
||||
// Since we use dense matrices Householder QR might be better:
|
||||
// https://mathoverflow.net/questions/227543/why-householder-reflection-is-better-than-givens-rotation-in-dense-linear-algebr
|
||||
|
||||
if (options_->use_householder) {
|
||||
performQRHouseholder();
|
||||
} else {
|
||||
performQRGivens();
|
||||
}
|
||||
|
||||
state = State::Marginalized;
|
||||
}
|
||||
|
||||
// Sets damping and maintains upper triangular matrix for landmarks.
|
||||
virtual inline void setLandmarkDamping(Scalar lambda) override {
|
||||
BASALT_ASSERT(state == State::Marginalized);
|
||||
BASALT_ASSERT(lambda >= 0);
|
||||
|
||||
if (hasLandmarkDamping()) {
|
||||
BASALT_ASSERT(damping_rotations.size() == 6);
|
||||
|
||||
// undo dampening
|
||||
for (int n = 2; n >= 0; n--) {
|
||||
for (int m = n; m >= 0; m--) {
|
||||
storage.applyOnTheLeft(num_rows - 3 + n - m, n,
|
||||
damping_rotations.back().adjoint());
|
||||
damping_rotations.pop_back();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (lambda == 0) {
|
||||
storage.template block<3, 3>(num_rows - 3, lm_idx).diagonal().setZero();
|
||||
} else {
|
||||
BASALT_ASSERT(Jl_col_scale.array().isFinite().all());
|
||||
|
||||
storage.template block<3, 3>(num_rows - 3, lm_idx)
|
||||
.diagonal()
|
||||
.setConstant(sqrt(lambda));
|
||||
|
||||
BASALT_ASSERT(damping_rotations.empty());
|
||||
|
||||
// apply dampening and remember rotations to undo
|
||||
for (int n = 0; n < 3; n++) {
|
||||
for (int m = 0; m <= n; m++) {
|
||||
damping_rotations.emplace_back();
|
||||
damping_rotations.back().makeGivens(
|
||||
storage(n, lm_idx + n),
|
||||
storage(num_rows - 3 + n - m, lm_idx + n));
|
||||
storage.applyOnTheLeft(num_rows - 3 + n - m, n,
|
||||
damping_rotations.back());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// lambda < 0 means computing exact model cost change
|
||||
virtual inline void backSubstitute(const VecX& pose_inc,
|
||||
Scalar& l_diff) override {
|
||||
BASALT_ASSERT(state == State::Marginalized);
|
||||
|
||||
// For now we include all columns in LMB
|
||||
BASALT_ASSERT(pose_inc.size() == signed_cast(padding_idx));
|
||||
|
||||
const auto Q1Jl = storage.template block<3, 3>(0, lm_idx)
|
||||
.template triangularView<Eigen::Upper>();
|
||||
|
||||
const auto Q1Jr = storage.col(res_idx).template head<3>();
|
||||
const auto Q1Jp = storage.topLeftCorner(3, padding_idx);
|
||||
|
||||
Vec3 inc = -Q1Jl.solve(Q1Jr + Q1Jp * pose_inc);
|
||||
|
||||
// We want to compute the model cost change. The model function is
|
||||
//
|
||||
// L(inc) = F(x) + inc^T J^T r + 0.5 inc^T J^T J inc
|
||||
//
|
||||
// and thus the expected decrease in cost for the computed increment is
|
||||
//
|
||||
// l_diff = L(0) - L(inc)
|
||||
// = - inc^T J^T r - 0.5 inc^T J^T J inc
|
||||
// = - inc^T J^T (r + 0.5 J inc)
|
||||
// = - (J inc)^T (r + 0.5 (J inc)).
|
||||
//
|
||||
// Here we have J = [Jp, Jl] under the orthogonal projection Q = [Q1, Q2],
|
||||
// i.e. the linearized system (model cost) is
|
||||
//
|
||||
// L(inc) = 0.5 || J inc + r ||^2 = 0.5 || Q^T J inc + Q^T r ||^2
|
||||
//
|
||||
// and below we thus compute
|
||||
//
|
||||
// l_diff = - (Q^T J inc)^T (Q^T r + 0.5 (Q^T J inc)).
|
||||
//
|
||||
// We have
|
||||
// | Q1^T | | Q1^T Jp Q1^T Jl |
|
||||
// Q^T J = | | [Jp, Jl] = | |
|
||||
// | Q2^T | | Q2^T Jp 0 |.
|
||||
//
|
||||
// Note that Q2 is the nullspace of Jl, and Q1^T Jl == R. So with inc =
|
||||
// [incp^T, incl^T]^T we have
|
||||
//
|
||||
// | Q1^T Jp incp + Q1^T Jl incl |
|
||||
// Q^T J inc = | |
|
||||
// | Q2^T Jp incp |
|
||||
//
|
||||
|
||||
// undo damping before we compute the model cost difference
|
||||
setLandmarkDamping(0);
|
||||
|
||||
// compute "Q^T J incp"
|
||||
VecX QJinc = storage.topLeftCorner(num_rows - 3, padding_idx) * pose_inc;
|
||||
|
||||
// add "Q1^T Jl incl" to the first 3 rows
|
||||
QJinc.template head<3>() += Q1Jl * inc;
|
||||
|
||||
auto Qr = storage.col(res_idx).head(num_rows - 3);
|
||||
l_diff -= QJinc.transpose() * (Scalar(0.5) * QJinc + Qr);
|
||||
|
||||
// TODO: detect and handle case like ceres, allowing a few iterations but
|
||||
// stopping eventually
|
||||
if (!inc.array().isFinite().all() ||
|
||||
!lm_ptr->direction.array().isFinite().all() ||
|
||||
!std::isfinite(lm_ptr->inv_dist)) {
|
||||
std::cerr << "Numerical failure in backsubstitution\n";
|
||||
}
|
||||
|
||||
// Note: scale only after computing model cost change
|
||||
inc.array() *= Jl_col_scale.array();
|
||||
|
||||
lm_ptr->direction += inc.template head<2>();
|
||||
lm_ptr->inv_dist = std::max(Scalar(0), lm_ptr->inv_dist + inc[2]);
|
||||
}
|
||||
|
||||
virtual inline size_t numReducedCams() const override {
|
||||
BASALT_LOG_FATAL("check what we mean by numReducedCams for absolute poses");
|
||||
return pose_lin_vec.size();
|
||||
}
|
||||
|
||||
inline void addQ2JpTQ2Jp_mult_x(VecX& res,
|
||||
const VecX& x_pose) const override {
|
||||
UNUSED(res);
|
||||
UNUSED(x_pose);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
virtual inline void addQ2JpTQ2r(VecX& res) const override {
|
||||
UNUSED(res);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
virtual inline void addJp_diag2(VecX& res) const override {
|
||||
BASALT_ASSERT(state == State::Linearized);
|
||||
|
||||
for (const auto& [frame_id, idx_set] : res_idx_by_abs_pose_) {
|
||||
const int pose_idx = aom_->abs_order_map.at(frame_id).first;
|
||||
for (const int i : idx_set) {
|
||||
const auto block = storage.block(2 * i, pose_idx, 2, POSE_SIZE);
|
||||
|
||||
res.template segment<POSE_SIZE>(pose_idx) +=
|
||||
block.colwise().squaredNorm();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual inline void addQ2JpTQ2Jp_blockdiag(
|
||||
BlockDiagonalAccumulator<Scalar>& accu) const override {
|
||||
UNUSED(accu);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
virtual inline void scaleJl_cols() override {
|
||||
BASALT_ASSERT(state == State::Linearized);
|
||||
|
||||
// ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
|
||||
// we use 1.0 / (eps + sqrt(SquaredColumnNorm))
|
||||
Jl_col_scale =
|
||||
(options_->jacobi_scaling_eps +
|
||||
storage.block(0, lm_idx, num_rows - 3, 3).colwise().norm().array())
|
||||
.inverse();
|
||||
|
||||
storage.block(0, lm_idx, num_rows - 3, 3) *= Jl_col_scale.asDiagonal();
|
||||
}
|
||||
|
||||
virtual inline void scaleJp_cols(const VecX& jacobian_scaling) override {
|
||||
BASALT_ASSERT(state == State::Marginalized);
|
||||
|
||||
// we assume we apply scaling before damping (we exclude the last 3 rows)
|
||||
BASALT_ASSERT(!hasLandmarkDamping());
|
||||
|
||||
storage.topLeftCorner(num_rows - 3, padding_idx) *=
|
||||
jacobian_scaling.asDiagonal();
|
||||
}
|
||||
|
||||
inline bool hasLandmarkDamping() const { return !damping_rotations.empty(); }
|
||||
|
||||
virtual inline void printStorage(const std::string& filename) const override {
|
||||
std::ofstream f(filename);
|
||||
|
||||
Eigen::IOFormat CleanFmt(4, 0, " ", "\n", "", "");
|
||||
|
||||
f << "Storage (state: " << state
|
||||
<< ", damping: " << (hasLandmarkDamping() ? "yes" : "no")
|
||||
<< " Jl_col_scale: " << Jl_col_scale.transpose() << "):\n"
|
||||
<< storage.format(CleanFmt) << std::endl;
|
||||
|
||||
f.close();
|
||||
}
|
||||
#if 0
|
||||
virtual inline void stage2(
|
||||
Scalar lambda, const VecX* jacobian_scaling, VecX* precond_diagonal2,
|
||||
BlockDiagonalAccumulator<Scalar>* precond_block_diagonal,
|
||||
VecX& bref) override {
|
||||
// 1. scale jacobian
|
||||
if (jacobian_scaling) {
|
||||
scaleJp_cols(*jacobian_scaling);
|
||||
}
|
||||
|
||||
// 2. dampen landmarks
|
||||
setLandmarkDamping(lambda);
|
||||
|
||||
// 3a. compute diagonal preconditioner (SCHUR_JACOBI_DIAGONAL)
|
||||
if (precond_diagonal2) {
|
||||
addQ2Jp_diag2(*precond_diagonal2);
|
||||
}
|
||||
|
||||
// 3b. compute block diagonal preconditioner (SCHUR_JACOBI)
|
||||
if (precond_block_diagonal) {
|
||||
addQ2JpTQ2Jp_blockdiag(*precond_block_diagonal);
|
||||
}
|
||||
|
||||
// 4. compute rhs of reduced camera normal equations
|
||||
addQ2JpTQ2r(bref);
|
||||
}
|
||||
#endif
|
||||
|
||||
inline State getState() const override { return state; }
|
||||
|
||||
virtual inline size_t numQ2rows() const override { return num_rows - 3; }
|
||||
|
||||
protected:
|
||||
inline void performQRGivens() {
|
||||
// Based on "Matrix Computations 4th Edition by Golub and Van Loan"
|
||||
// See page 252, Algorithm 5.2.4 for how these two loops work
|
||||
Eigen::JacobiRotation<Scalar> gr;
|
||||
for (size_t n = 0; n < 3; n++) {
|
||||
for (size_t m = num_rows - 4; m > n; m--) {
|
||||
gr.makeGivens(storage(m - 1, lm_idx + n), storage(m, lm_idx + n));
|
||||
storage.applyOnTheLeft(m, m - 1, gr);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void performQRHouseholder() {
|
||||
VecX tempVector1(num_cols);
|
||||
VecX tempVector2(num_rows - 3);
|
||||
|
||||
for (size_t k = 0; k < 3; ++k) {
|
||||
size_t remainingRows = num_rows - k - 3;
|
||||
|
||||
Scalar beta;
|
||||
Scalar tau;
|
||||
storage.col(lm_idx + k)
|
||||
.segment(k, remainingRows)
|
||||
.makeHouseholder(tempVector2, tau, beta);
|
||||
|
||||
storage.block(k, 0, remainingRows, num_cols)
|
||||
.applyHouseholderOnTheLeft(tempVector2, tau, tempVector1.data());
|
||||
}
|
||||
}
|
||||
|
||||
inline std::tuple<Scalar, Scalar> compute_error_weight(
|
||||
Scalar res_squared) const {
|
||||
// Note: Definition of cost is 0.5 ||r(x)||^2 to be in line with ceres
|
||||
|
||||
if (options_->huber_parameter > 0) {
|
||||
// use huber norm
|
||||
const Scalar huber_weight =
|
||||
res_squared <= options_->huber_parameter * options_->huber_parameter
|
||||
? Scalar(1)
|
||||
: options_->huber_parameter / std::sqrt(res_squared);
|
||||
const Scalar error =
|
||||
Scalar(0.5) * (2 - huber_weight) * huber_weight * res_squared;
|
||||
return {error, huber_weight};
|
||||
} else {
|
||||
// use squared norm
|
||||
return {Scalar(0.5) * res_squared, Scalar(1)};
|
||||
}
|
||||
}
|
||||
|
||||
void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
|
||||
size_t start_idx) const override {
|
||||
Q2r.segment(start_idx, num_rows - 3) =
|
||||
storage.col(res_idx).tail(num_rows - 3);
|
||||
|
||||
BASALT_ASSERT(Q2Jp.cols() == signed_cast(padding_idx));
|
||||
|
||||
Q2Jp.block(start_idx, 0, num_rows - 3, padding_idx) =
|
||||
storage.block(3, 0, num_rows - 3, padding_idx);
|
||||
}
|
||||
|
||||
void get_dense_Q2Jp_Q2r_rel(
|
||||
MatX& Q2Jp, VecX& Q2r, size_t start_idx,
|
||||
const std::map<TimeCamId, size_t>& rel_order) const override {
|
||||
UNUSED(Q2Jp);
|
||||
UNUSED(Q2r);
|
||||
UNUSED(start_idx);
|
||||
UNUSED(rel_order);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
void add_dense_H_b(DenseAccumulator<Scalar>& accum) const override {
|
||||
UNUSED(accum);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
void add_dense_H_b(MatX& H, VecX& b) const override {
|
||||
const auto r = storage.col(res_idx).tail(num_rows - 3);
|
||||
const auto J = storage.block(3, 0, num_rows - 3, padding_idx);
|
||||
|
||||
H.noalias() += J.transpose() * J;
|
||||
b.noalias() += J.transpose() * r;
|
||||
}
|
||||
|
||||
void add_dense_H_b_rel(
|
||||
MatX& H_rel, VecX& b_rel,
|
||||
const std::map<TimeCamId, size_t>& rel_order) const override {
|
||||
UNUSED(H_rel);
|
||||
UNUSED(b_rel);
|
||||
UNUSED(rel_order);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
|
||||
const override {
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
|
||||
const std::map<TimeCamId, size_t>& rel_order) const override {
|
||||
UNUSED(rel_order);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const override {
|
||||
UNUSED(H_rel);
|
||||
UNUSED(b_rel);
|
||||
BASALT_LOG_FATAL("not implemented");
|
||||
}
|
||||
|
||||
virtual TimeCamId getHostKf() const override { return lm_ptr->host_kf_id; }
|
||||
|
||||
private:
|
||||
// Dense storage for pose Jacobians, padding, landmark Jacobians and
|
||||
// residuals [J_p | pad | J_l | res]
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
|
||||
storage;
|
||||
|
||||
Vec3 Jl_col_scale = Vec3::Ones();
|
||||
std::vector<Eigen::JacobiRotation<Scalar>> damping_rotations;
|
||||
|
||||
std::vector<const RelPoseLin<Scalar>*> pose_lin_vec;
|
||||
std::vector<const std::pair<TimeCamId, TimeCamId>*> pose_tcid_vec;
|
||||
size_t padding_idx = 0;
|
||||
size_t padding_size = 0;
|
||||
size_t lm_idx = 0;
|
||||
size_t res_idx = 0;
|
||||
|
||||
size_t num_cols = 0;
|
||||
size_t num_rows = 0;
|
||||
|
||||
const Options* options_ = nullptr;
|
||||
|
||||
State state = State::Uninitialized;
|
||||
|
||||
Keypoint<Scalar>* lm_ptr = nullptr;
|
||||
const Calibration<Scalar>* calib_ = nullptr;
|
||||
const AbsOrderMap* aom_ = nullptr;
|
||||
|
||||
std::map<int64_t, std::set<int>> res_idx_by_abs_pose_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
141
include/basalt/linearization/linearization_abs_qr.hpp
Normal file
141
include/basalt/linearization/linearization_abs_qr.hpp
Normal file
@@ -0,0 +1,141 @@
|
||||
#pragma once
|
||||
|
||||
#include <basalt/linearization/linearization_base.hpp>
|
||||
|
||||
#include <basalt/optimization/accumulator.h>
|
||||
#include <basalt/vi_estimator/landmark_database.h>
|
||||
#include <basalt/linearization/landmark_block.hpp>
|
||||
#include <basalt/utils/cast_utils.hpp>
|
||||
#include <basalt/utils/time_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Scalar>
|
||||
class ImuBlock;
|
||||
|
||||
template <typename Scalar_, int POSE_SIZE_>
|
||||
class LinearizationAbsQR : public LinearizationBase<Scalar_, POSE_SIZE_> {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int POSE_SIZE = POSE_SIZE_;
|
||||
using Base = LinearizationBase<Scalar, POSE_SIZE>;
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
using VecP = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
|
||||
|
||||
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
|
||||
|
||||
using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
|
||||
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
|
||||
using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
|
||||
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
using LandmarkBlockPtr = std::unique_ptr<LandmarkBlock<Scalar>>;
|
||||
using ImuBlockPtr = std::unique_ptr<ImuBlock<Scalar>>;
|
||||
|
||||
using typename Base::Options;
|
||||
|
||||
LinearizationAbsQR(
|
||||
BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
|
||||
const Options& options,
|
||||
const MargLinData<Scalar>* marg_lin_data = nullptr,
|
||||
const ImuLinData<Scalar>* imu_lin_data = nullptr,
|
||||
const std::set<FrameId>* used_frames = nullptr,
|
||||
const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
|
||||
int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
|
||||
|
||||
// destructor defined in cpp b/c of unique_ptr destructor (ImuBlock)
|
||||
// copy/move constructor/assignment-operator for rule-of-five
|
||||
~LinearizationAbsQR();
|
||||
LinearizationAbsQR(const LinearizationAbsQR&) = default;
|
||||
LinearizationAbsQR(LinearizationAbsQR&&) = default;
|
||||
LinearizationAbsQR& operator=(const LinearizationAbsQR&) = default;
|
||||
LinearizationAbsQR& operator=(LinearizationAbsQR&&) = default;
|
||||
|
||||
void log_problem_stats(ExecutionStats& stats) const override;
|
||||
|
||||
Scalar linearizeProblem(bool* numerically_valid = nullptr) override;
|
||||
|
||||
void performQR() override;
|
||||
|
||||
void setPoseDamping(const Scalar lambda);
|
||||
|
||||
bool hasPoseDamping() const { return pose_damping_diagonal > 0; }
|
||||
|
||||
Scalar backSubstitute(const VecX& pose_inc) override;
|
||||
|
||||
VecX getJp_diag2() const;
|
||||
|
||||
void scaleJl_cols();
|
||||
|
||||
void scaleJp_cols(const VecX& jacobian_scaling);
|
||||
|
||||
void setLandmarkDamping(Scalar lambda);
|
||||
|
||||
void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const override;
|
||||
|
||||
void get_dense_H_b(MatX& H, VecX& b) const override;
|
||||
|
||||
protected: // types
|
||||
using PoseLinMapType =
|
||||
Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
|
||||
RelPoseLin<Scalar>>;
|
||||
using PoseLinMapTypeConstIter = typename PoseLinMapType::const_iterator;
|
||||
|
||||
using HostLandmarkMapType =
|
||||
std::unordered_map<TimeCamId, std::vector<const LandmarkBlock<Scalar>*>>;
|
||||
|
||||
protected: // helper
|
||||
void get_dense_Q2Jp_Q2r_pose_damping(MatX& Q2Jp, size_t start_idx) const;
|
||||
|
||||
void get_dense_Q2Jp_Q2r_marg_prior(MatX& Q2Jp, VecX& Q2r,
|
||||
size_t start_idx) const;
|
||||
|
||||
void add_dense_H_b_pose_damping(MatX& H) const;
|
||||
|
||||
void add_dense_H_b_marg_prior(MatX& H, VecX& b) const;
|
||||
|
||||
void add_dense_H_b_imu(DenseAccumulator<Scalar>& accum) const;
|
||||
|
||||
void add_dense_H_b_imu(MatX& H, VecX& b) const;
|
||||
|
||||
protected:
|
||||
Options options_;
|
||||
|
||||
std::vector<KeypointId> landmark_ids;
|
||||
std::vector<LandmarkBlockPtr> landmark_blocks;
|
||||
std::vector<ImuBlockPtr> imu_blocks;
|
||||
|
||||
std::unordered_map<TimeCamId, size_t> host_to_idx_;
|
||||
HostLandmarkMapType host_to_landmark_block;
|
||||
|
||||
std::vector<size_t> landmark_block_idx;
|
||||
const BundleAdjustmentBase<Scalar>* estimator;
|
||||
|
||||
LandmarkDatabase<Scalar>& lmdb_;
|
||||
const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses;
|
||||
|
||||
const Calibration<Scalar>& calib;
|
||||
|
||||
const AbsOrderMap& aom;
|
||||
const std::set<FrameId>* used_frames;
|
||||
|
||||
const MargLinData<Scalar>* marg_lin_data;
|
||||
const ImuLinData<Scalar>* imu_lin_data;
|
||||
|
||||
PoseLinMapType relative_pose_lin;
|
||||
std::vector<std::vector<PoseLinMapTypeConstIter>> relative_pose_per_host;
|
||||
|
||||
Scalar pose_damping_diagonal;
|
||||
Scalar pose_damping_diagonal_sqrt;
|
||||
|
||||
VecX marg_scaling;
|
||||
|
||||
size_t num_cameras;
|
||||
size_t num_rows_Q2r;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
142
include/basalt/linearization/linearization_abs_sc.hpp
Normal file
142
include/basalt/linearization/linearization_abs_sc.hpp
Normal file
@@ -0,0 +1,142 @@
|
||||
#pragma once
|
||||
|
||||
#include <basalt/linearization/linearization_base.hpp>
|
||||
|
||||
#include <basalt/optimization/accumulator.h>
|
||||
#include <basalt/vi_estimator/landmark_database.h>
|
||||
#include <basalt/vi_estimator/sqrt_ba_base.h>
|
||||
#include <basalt/linearization/landmark_block.hpp>
|
||||
#include <basalt/utils/cast_utils.hpp>
|
||||
#include <basalt/utils/time_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Scalar>
|
||||
class ImuBlock;
|
||||
|
||||
template <typename Scalar_, int POSE_SIZE_>
|
||||
class LinearizationAbsSC : public LinearizationBase<Scalar_, POSE_SIZE_> {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int POSE_SIZE = POSE_SIZE_;
|
||||
using Base = LinearizationBase<Scalar, POSE_SIZE>;
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
using VecP = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
|
||||
|
||||
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
|
||||
|
||||
using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
|
||||
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
|
||||
using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
|
||||
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
using LandmarkBlockPtr = std::unique_ptr<LandmarkBlock<Scalar>>;
|
||||
using ImuBlockPtr = std::unique_ptr<ImuBlock<Scalar>>;
|
||||
|
||||
using AbsLinData = typename ScBundleAdjustmentBase<Scalar>::AbsLinData;
|
||||
|
||||
using typename Base::Options;
|
||||
|
||||
LinearizationAbsSC(
|
||||
BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
|
||||
const Options& options,
|
||||
const MargLinData<Scalar>* marg_lin_data = nullptr,
|
||||
const ImuLinData<Scalar>* imu_lin_data = nullptr,
|
||||
const std::set<FrameId>* used_frames = nullptr,
|
||||
const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
|
||||
int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
|
||||
|
||||
// destructor defined in cpp b/c of unique_ptr destructor (ImuBlock)
|
||||
// copy/move constructor/assignment-operator for rule-of-five
|
||||
~LinearizationAbsSC();
|
||||
LinearizationAbsSC(const LinearizationAbsSC&) = default;
|
||||
LinearizationAbsSC(LinearizationAbsSC&&) = default;
|
||||
LinearizationAbsSC& operator=(const LinearizationAbsSC&) = default;
|
||||
LinearizationAbsSC& operator=(LinearizationAbsSC&&) = default;
|
||||
|
||||
void log_problem_stats(ExecutionStats& stats) const override;
|
||||
|
||||
Scalar linearizeProblem(bool* numerically_valid = nullptr) override;
|
||||
|
||||
void performQR() override;
|
||||
|
||||
void setPoseDamping(const Scalar lambda);
|
||||
|
||||
bool hasPoseDamping() const { return pose_damping_diagonal > 0; }
|
||||
|
||||
Scalar backSubstitute(const VecX& pose_inc) override;
|
||||
|
||||
VecX getJp_diag2() const;
|
||||
|
||||
void scaleJl_cols();
|
||||
|
||||
void scaleJp_cols(const VecX& jacobian_scaling);
|
||||
|
||||
void setLandmarkDamping(Scalar lambda);
|
||||
|
||||
void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const override;
|
||||
|
||||
void get_dense_H_b(MatX& H, VecX& b) const override;
|
||||
|
||||
protected: // types
|
||||
using PoseLinMapType =
|
||||
Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
|
||||
RelPoseLin<Scalar>>;
|
||||
using PoseLinMapTypeConstIter = typename PoseLinMapType::const_iterator;
|
||||
|
||||
using HostLandmarkMapType =
|
||||
std::unordered_map<TimeCamId, std::vector<const LandmarkBlock<Scalar>*>>;
|
||||
|
||||
protected: // helper
|
||||
void get_dense_Q2Jp_Q2r_pose_damping(MatX& Q2Jp, size_t start_idx) const;
|
||||
|
||||
void get_dense_Q2Jp_Q2r_marg_prior(MatX& Q2Jp, VecX& Q2r,
|
||||
size_t start_idx) const;
|
||||
|
||||
void add_dense_H_b_pose_damping(MatX& H) const;
|
||||
|
||||
void add_dense_H_b_marg_prior(MatX& H, VecX& b) const;
|
||||
|
||||
void add_dense_H_b_imu(DenseAccumulator<Scalar>& accum) const;
|
||||
|
||||
void add_dense_H_b_imu(MatX& H, VecX& b) const;
|
||||
|
||||
// Transform to abs
|
||||
static void accumulate_dense_H_b_rel_to_abs(
|
||||
const MatX& rel_H, const VecX& rel_b,
|
||||
const std::vector<PoseLinMapTypeConstIter>& rpph, const AbsOrderMap& aom,
|
||||
DenseAccumulator<Scalar>& accum);
|
||||
|
||||
protected:
|
||||
Options options_;
|
||||
|
||||
std::vector<ImuBlockPtr> imu_blocks;
|
||||
|
||||
const BundleAdjustmentBase<Scalar>* estimator;
|
||||
|
||||
LandmarkDatabase<Scalar>& lmdb_;
|
||||
const Calibration<Scalar>& calib;
|
||||
|
||||
const AbsOrderMap& aom;
|
||||
const std::set<FrameId>* used_frames;
|
||||
|
||||
const MargLinData<Scalar>* marg_lin_data;
|
||||
const ImuLinData<Scalar>* imu_lin_data;
|
||||
const std::unordered_set<KeypointId>* lost_landmarks;
|
||||
int64_t last_state_to_marg;
|
||||
|
||||
Eigen::aligned_vector<AbsLinData> ald_vec;
|
||||
|
||||
Scalar pose_damping_diagonal;
|
||||
Scalar pose_damping_diagonal_sqrt;
|
||||
|
||||
VecX marg_scaling;
|
||||
|
||||
size_t num_rows_Q2r;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
63
include/basalt/linearization/linearization_base.hpp
Normal file
63
include/basalt/linearization/linearization_base.hpp
Normal file
@@ -0,0 +1,63 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <basalt/vi_estimator/ba_base.h>
|
||||
#include <basalt/linearization/landmark_block.hpp>
|
||||
#include <basalt/utils/time_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <typename Scalar_, int POSE_SIZE_>
|
||||
class LinearizationBase {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int POSE_SIZE = POSE_SIZE_;
|
||||
|
||||
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
struct Options {
|
||||
typename LandmarkBlock<Scalar>::Options lb_options;
|
||||
LinearizationType linearization_type;
|
||||
};
|
||||
|
||||
virtual ~LinearizationBase() = default;
|
||||
|
||||
virtual void log_problem_stats(ExecutionStats& stats) const = 0;
|
||||
|
||||
virtual Scalar linearizeProblem(bool* numerically_valid = nullptr) = 0;
|
||||
|
||||
virtual void performQR() = 0;
|
||||
|
||||
// virtual void setPoseDamping(const Scalar lambda) = 0;
|
||||
|
||||
// virtual bool hasPoseDamping() const = 0;
|
||||
|
||||
virtual Scalar backSubstitute(const VecX& pose_inc) = 0;
|
||||
|
||||
// virtual VecX getJp_diag2() const = 0;
|
||||
|
||||
// virtual void scaleJl_cols() = 0;
|
||||
|
||||
// virtual void scaleJp_cols(const VecX& jacobian_scaling) = 0;
|
||||
|
||||
// virtual void setLandmarkDamping(Scalar lambda) = 0;
|
||||
|
||||
virtual void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const = 0;
|
||||
|
||||
virtual void get_dense_H_b(MatX& H, VecX& b) const = 0;
|
||||
|
||||
static std::unique_ptr<LinearizationBase> create(
|
||||
BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
|
||||
const Options& options,
|
||||
const MargLinData<Scalar>* marg_lin_data = nullptr,
|
||||
const ImuLinData<Scalar>* imu_lin_data = nullptr,
|
||||
const std::set<FrameId>* used_frames = nullptr,
|
||||
const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
|
||||
int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
|
||||
};
|
||||
|
||||
bool isLinearizationSqrt(const LinearizationType& type);
|
||||
|
||||
} // namespace basalt
|
||||
143
include/basalt/linearization/linearization_rel_sc.hpp
Normal file
143
include/basalt/linearization/linearization_rel_sc.hpp
Normal file
@@ -0,0 +1,143 @@
|
||||
#pragma once
|
||||
|
||||
#include <basalt/linearization/linearization_base.hpp>
|
||||
|
||||
#include <basalt/optimization/accumulator.h>
|
||||
#include <basalt/vi_estimator/landmark_database.h>
|
||||
#include <basalt/vi_estimator/sqrt_ba_base.h>
|
||||
#include <basalt/linearization/landmark_block.hpp>
|
||||
#include <basalt/utils/cast_utils.hpp>
|
||||
#include <basalt/utils/time_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Scalar>
|
||||
class ImuBlock;
|
||||
|
||||
template <typename Scalar_, int POSE_SIZE_>
|
||||
class LinearizationRelSC : public LinearizationBase<Scalar_, POSE_SIZE_> {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
static constexpr int POSE_SIZE = POSE_SIZE_;
|
||||
using Base = LinearizationBase<Scalar, POSE_SIZE>;
|
||||
|
||||
using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
|
||||
using VecP = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
|
||||
|
||||
using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
|
||||
|
||||
using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
|
||||
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
|
||||
using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
|
||||
|
||||
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
using LandmarkBlockPtr = std::unique_ptr<LandmarkBlock<Scalar>>;
|
||||
using ImuBlockPtr = std::unique_ptr<ImuBlock<Scalar>>;
|
||||
|
||||
using RelLinData = typename ScBundleAdjustmentBase<Scalar>::RelLinData;
|
||||
|
||||
using typename Base::Options;
|
||||
|
||||
LinearizationRelSC(
|
||||
BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
|
||||
const Options& options,
|
||||
const MargLinData<Scalar>* marg_lin_data = nullptr,
|
||||
const ImuLinData<Scalar>* imu_lin_data = nullptr,
|
||||
const std::set<FrameId>* used_frames = nullptr,
|
||||
const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
|
||||
int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
|
||||
|
||||
// destructor defined in cpp b/c of unique_ptr destructor (ImuBlock)
|
||||
// copy/move constructor/assignment-operator for rule-of-five
|
||||
~LinearizationRelSC();
|
||||
LinearizationRelSC(const LinearizationRelSC&) = default;
|
||||
LinearizationRelSC(LinearizationRelSC&&) = default;
|
||||
LinearizationRelSC& operator=(const LinearizationRelSC&) = default;
|
||||
LinearizationRelSC& operator=(LinearizationRelSC&&) = default;
|
||||
|
||||
void log_problem_stats(ExecutionStats& stats) const override;
|
||||
|
||||
Scalar linearizeProblem(bool* numerically_valid = nullptr) override;
|
||||
|
||||
void performQR() override;
|
||||
|
||||
void setPoseDamping(const Scalar lambda);
|
||||
|
||||
bool hasPoseDamping() const { return pose_damping_diagonal > 0; }
|
||||
|
||||
Scalar backSubstitute(const VecX& pose_inc) override;
|
||||
|
||||
VecX getJp_diag2() const;
|
||||
|
||||
void scaleJl_cols();
|
||||
|
||||
void scaleJp_cols(const VecX& jacobian_scaling);
|
||||
|
||||
void setLandmarkDamping(Scalar lambda);
|
||||
|
||||
void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const override;
|
||||
|
||||
void get_dense_H_b(MatX& H, VecX& b) const override;
|
||||
|
||||
protected: // types
|
||||
using PoseLinMapType =
|
||||
Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
|
||||
RelPoseLin<Scalar>>;
|
||||
using PoseLinMapTypeConstIter = typename PoseLinMapType::const_iterator;
|
||||
|
||||
using HostLandmarkMapType =
|
||||
std::unordered_map<TimeCamId, std::vector<const LandmarkBlock<Scalar>*>>;
|
||||
|
||||
protected: // helper
|
||||
void get_dense_Q2Jp_Q2r_pose_damping(MatX& Q2Jp, size_t start_idx) const;
|
||||
|
||||
void get_dense_Q2Jp_Q2r_marg_prior(MatX& Q2Jp, VecX& Q2r,
|
||||
size_t start_idx) const;
|
||||
|
||||
void add_dense_H_b_pose_damping(MatX& H) const;
|
||||
|
||||
void add_dense_H_b_marg_prior(MatX& H, VecX& b) const;
|
||||
|
||||
void add_dense_H_b_imu(DenseAccumulator<Scalar>& accum) const;
|
||||
|
||||
void add_dense_H_b_imu(MatX& H, VecX& b) const;
|
||||
|
||||
// Transform to abs
|
||||
static void accumulate_dense_H_b_rel_to_abs(
|
||||
const MatX& rel_H, const VecX& rel_b,
|
||||
const std::vector<PoseLinMapTypeConstIter>& rpph, const AbsOrderMap& aom,
|
||||
DenseAccumulator<Scalar>& accum);
|
||||
|
||||
protected:
|
||||
Options options_;
|
||||
|
||||
std::vector<ImuBlockPtr> imu_blocks;
|
||||
|
||||
const BundleAdjustmentBase<Scalar>* estimator;
|
||||
|
||||
LandmarkDatabase<Scalar>& lmdb_;
|
||||
|
||||
const Calibration<Scalar>& calib;
|
||||
|
||||
const AbsOrderMap& aom;
|
||||
const std::set<FrameId>* used_frames;
|
||||
|
||||
const MargLinData<Scalar>* marg_lin_data;
|
||||
const ImuLinData<Scalar>* imu_lin_data;
|
||||
const std::unordered_set<KeypointId>* lost_landmarks;
|
||||
int64_t last_state_to_marg;
|
||||
|
||||
Eigen::aligned_vector<RelLinData> rld_vec;
|
||||
|
||||
Scalar pose_damping_diagonal;
|
||||
Scalar pose_damping_diagonal_sqrt;
|
||||
|
||||
VecX marg_scaling;
|
||||
|
||||
size_t num_rows_Q2r;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
405
include/basalt/optical_flow/frame_to_frame_optical_flow.h
Normal file
405
include/basalt/optical_flow/frame_to_frame_optical_flow.h
Normal file
@@ -0,0 +1,405 @@
|
||||
/**
|
||||
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 <sophus/se2.hpp>
|
||||
|
||||
#include <tbb/blocked_range.h>
|
||||
#include <tbb/concurrent_unordered_map.h>
|
||||
#include <tbb/parallel_for.h>
|
||||
|
||||
#include <basalt/optical_flow/optical_flow.h>
|
||||
#include <basalt/optical_flow/patch.h>
|
||||
|
||||
#include <basalt/image/image_pyr.h>
|
||||
#include <basalt/utils/keypoints.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
/// Unlike PatchOpticalFlow, FrameToFrameOpticalFlow always tracks patches
|
||||
/// against the previous frame, not the initial frame where a track was created.
|
||||
/// While it might cause more drift of the patch location, it leads to longer
|
||||
/// tracks in practice.
|
||||
template <typename Scalar, template <typename> typename Pattern>
|
||||
class FrameToFrameOpticalFlow : public OpticalFlowBase {
|
||||
public:
|
||||
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
|
||||
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
|
||||
|
||||
typedef Sophus::SE2<Scalar> SE2;
|
||||
|
||||
FrameToFrameOpticalFlow(const VioConfig& config,
|
||||
const basalt::Calibration<double>& calib)
|
||||
: t_ns(-1), frame_counter(0), last_keypoint_id(0), config(config) {
|
||||
input_queue.set_capacity(10);
|
||||
|
||||
this->calib = calib.cast<Scalar>();
|
||||
|
||||
patch_coord = PatchT::pattern2.template cast<float>();
|
||||
|
||||
if (calib.intrinsics.size() > 1) {
|
||||
Eigen::Matrix4d Ed;
|
||||
Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
|
||||
computeEssential(T_i_j, Ed);
|
||||
E = Ed.cast<Scalar>();
|
||||
}
|
||||
|
||||
processing_thread.reset(
|
||||
new std::thread(&FrameToFrameOpticalFlow::processingLoop, this));
|
||||
}
|
||||
|
||||
~FrameToFrameOpticalFlow() { processing_thread->join(); }
|
||||
|
||||
void processingLoop() {
|
||||
OpticalFlowInput::Ptr input_ptr;
|
||||
|
||||
while (true) {
|
||||
input_queue.pop(input_ptr);
|
||||
|
||||
if (!input_ptr.get()) {
|
||||
if (output_queue) output_queue->push(nullptr);
|
||||
break;
|
||||
}
|
||||
|
||||
processFrame(input_ptr->t_ns, input_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
void processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
|
||||
for (const auto& v : new_img_vec->img_data) {
|
||||
if (!v.img.get()) return;
|
||||
}
|
||||
|
||||
if (t_ns < 0) {
|
||||
t_ns = curr_t_ns;
|
||||
|
||||
transforms.reset(new OpticalFlowResult);
|
||||
transforms->observations.resize(calib.intrinsics.size());
|
||||
transforms->t_ns = t_ns;
|
||||
|
||||
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
|
||||
pyramid->resize(calib.intrinsics.size());
|
||||
|
||||
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
|
||||
[&](const tbb::blocked_range<size_t>& r) {
|
||||
for (size_t i = r.begin(); i != r.end(); ++i) {
|
||||
pyramid->at(i).setFromImage(
|
||||
*new_img_vec->img_data[i].img,
|
||||
config.optical_flow_levels);
|
||||
}
|
||||
});
|
||||
|
||||
transforms->input_images = new_img_vec;
|
||||
|
||||
addPoints();
|
||||
filterPoints();
|
||||
|
||||
} else {
|
||||
t_ns = curr_t_ns;
|
||||
|
||||
old_pyramid = pyramid;
|
||||
|
||||
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
|
||||
pyramid->resize(calib.intrinsics.size());
|
||||
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
|
||||
[&](const tbb::blocked_range<size_t>& r) {
|
||||
for (size_t i = r.begin(); i != r.end(); ++i) {
|
||||
pyramid->at(i).setFromImage(
|
||||
*new_img_vec->img_data[i].img,
|
||||
config.optical_flow_levels);
|
||||
}
|
||||
});
|
||||
|
||||
OpticalFlowResult::Ptr new_transforms;
|
||||
new_transforms.reset(new OpticalFlowResult);
|
||||
new_transforms->observations.resize(calib.intrinsics.size());
|
||||
new_transforms->t_ns = t_ns;
|
||||
|
||||
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
|
||||
trackPoints(old_pyramid->at(i), pyramid->at(i),
|
||||
transforms->observations[i],
|
||||
new_transforms->observations[i]);
|
||||
}
|
||||
|
||||
transforms = new_transforms;
|
||||
transforms->input_images = new_img_vec;
|
||||
|
||||
addPoints();
|
||||
filterPoints();
|
||||
}
|
||||
|
||||
if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) {
|
||||
output_queue->push(transforms);
|
||||
}
|
||||
|
||||
frame_counter++;
|
||||
}
|
||||
|
||||
void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1,
|
||||
const basalt::ManagedImagePyr<uint16_t>& pyr_2,
|
||||
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
|
||||
transform_map_1,
|
||||
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
|
||||
transform_map_2) const {
|
||||
size_t num_points = transform_map_1.size();
|
||||
|
||||
std::vector<KeypointId> ids;
|
||||
Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
|
||||
|
||||
ids.reserve(num_points);
|
||||
init_vec.reserve(num_points);
|
||||
|
||||
for (const auto& kv : transform_map_1) {
|
||||
ids.push_back(kv.first);
|
||||
init_vec.push_back(kv.second);
|
||||
}
|
||||
|
||||
tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
|
||||
std::hash<KeypointId>>
|
||||
result;
|
||||
|
||||
auto compute_func = [&](const tbb::blocked_range<size_t>& range) {
|
||||
for (size_t r = range.begin(); r != range.end(); ++r) {
|
||||
const KeypointId id = ids[r];
|
||||
|
||||
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
|
||||
Eigen::AffineCompact2f transform_2 = transform_1;
|
||||
|
||||
bool valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2);
|
||||
|
||||
if (valid) {
|
||||
Eigen::AffineCompact2f transform_1_recovered = transform_2;
|
||||
|
||||
valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered);
|
||||
|
||||
if (valid) {
|
||||
Scalar dist2 = (transform_1.translation() -
|
||||
transform_1_recovered.translation())
|
||||
.squaredNorm();
|
||||
|
||||
if (dist2 < config.optical_flow_max_recovered_dist2) {
|
||||
result[id] = transform_2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
tbb::blocked_range<size_t> range(0, num_points);
|
||||
|
||||
tbb::parallel_for(range, compute_func);
|
||||
// compute_func(range);
|
||||
|
||||
transform_map_2.clear();
|
||||
transform_map_2.insert(result.begin(), result.end());
|
||||
}
|
||||
|
||||
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& old_pyr,
|
||||
const basalt::ManagedImagePyr<uint16_t>& pyr,
|
||||
const Eigen::AffineCompact2f& old_transform,
|
||||
Eigen::AffineCompact2f& transform) const {
|
||||
bool patch_valid = true;
|
||||
|
||||
transform.linear().setIdentity();
|
||||
|
||||
for (int level = config.optical_flow_levels; level >= 0 && patch_valid;
|
||||
level--) {
|
||||
const Scalar scale = 1 << level;
|
||||
|
||||
transform.translation() /= scale;
|
||||
|
||||
PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
|
||||
|
||||
patch_valid &= p.valid;
|
||||
if (patch_valid) {
|
||||
// Perform tracking on current level
|
||||
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform);
|
||||
}
|
||||
|
||||
transform.translation() *= scale;
|
||||
}
|
||||
|
||||
transform.linear() = old_transform.linear() * transform.linear();
|
||||
|
||||
return patch_valid;
|
||||
}
|
||||
|
||||
inline bool trackPointAtLevel(const Image<const uint16_t>& img_2,
|
||||
const PatchT& dp,
|
||||
Eigen::AffineCompact2f& transform) const {
|
||||
bool patch_valid = true;
|
||||
|
||||
for (int iteration = 0;
|
||||
patch_valid && iteration < config.optical_flow_max_iterations;
|
||||
iteration++) {
|
||||
typename PatchT::VectorP res;
|
||||
|
||||
typename PatchT::Matrix2P transformed_pat =
|
||||
transform.linear().matrix() * PatchT::pattern2;
|
||||
transformed_pat.colwise() += transform.translation();
|
||||
|
||||
patch_valid &= dp.residual(img_2, transformed_pat, res);
|
||||
|
||||
if (patch_valid) {
|
||||
const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
|
||||
|
||||
// avoid NaN in increment (leads to SE2::exp crashing)
|
||||
patch_valid &= inc.array().isFinite().all();
|
||||
|
||||
// avoid very large increment
|
||||
patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
|
||||
|
||||
if (patch_valid) {
|
||||
transform *= SE2::exp(inc).matrix();
|
||||
|
||||
const int filter_margin = 2;
|
||||
|
||||
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return patch_valid;
|
||||
}
|
||||
|
||||
void addPoints() {
|
||||
Eigen::aligned_vector<Eigen::Vector2d> pts0;
|
||||
|
||||
for (const auto& kv : transforms->observations.at(0)) {
|
||||
pts0.emplace_back(kv.second.translation().cast<double>());
|
||||
}
|
||||
|
||||
KeypointsData kd;
|
||||
|
||||
detectKeypoints(pyramid->at(0).lvl(0), kd,
|
||||
config.optical_flow_detection_grid_size, 1, pts0);
|
||||
|
||||
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses0,
|
||||
new_poses1;
|
||||
|
||||
for (size_t i = 0; i < kd.corners.size(); i++) {
|
||||
Eigen::AffineCompact2f transform;
|
||||
transform.setIdentity();
|
||||
transform.translation() = kd.corners[i].cast<Scalar>();
|
||||
|
||||
transforms->observations.at(0)[last_keypoint_id] = transform;
|
||||
new_poses0[last_keypoint_id] = transform;
|
||||
|
||||
last_keypoint_id++;
|
||||
}
|
||||
|
||||
if (calib.intrinsics.size() > 1) {
|
||||
trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1);
|
||||
|
||||
for (const auto& kv : new_poses1) {
|
||||
transforms->observations.at(1).emplace(kv);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void filterPoints() {
|
||||
if (calib.intrinsics.size() < 2) return;
|
||||
|
||||
std::set<KeypointId> lm_to_remove;
|
||||
|
||||
std::vector<KeypointId> kpid;
|
||||
Eigen::aligned_vector<Eigen::Vector2f> proj0, proj1;
|
||||
|
||||
for (const auto& kv : transforms->observations.at(1)) {
|
||||
auto it = transforms->observations.at(0).find(kv.first);
|
||||
|
||||
if (it != transforms->observations.at(0).end()) {
|
||||
proj0.emplace_back(it->second.translation());
|
||||
proj1.emplace_back(kv.second.translation());
|
||||
kpid.emplace_back(kv.first);
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector4f> p3d0, p3d1;
|
||||
std::vector<bool> p3d0_success, p3d1_success;
|
||||
|
||||
calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success);
|
||||
calib.intrinsics[1].unproject(proj1, p3d1, p3d1_success);
|
||||
|
||||
for (size_t i = 0; i < p3d0_success.size(); i++) {
|
||||
if (p3d0_success[i] && p3d1_success[i]) {
|
||||
const double epipolar_error =
|
||||
std::abs(p3d0[i].transpose() * E * p3d1[i]);
|
||||
|
||||
if (epipolar_error > config.optical_flow_epipolar_error) {
|
||||
lm_to_remove.emplace(kpid[i]);
|
||||
}
|
||||
} else {
|
||||
lm_to_remove.emplace(kpid[i]);
|
||||
}
|
||||
}
|
||||
|
||||
for (int id : lm_to_remove) {
|
||||
transforms->observations.at(1).erase(id);
|
||||
}
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
int64_t t_ns;
|
||||
|
||||
size_t frame_counter;
|
||||
|
||||
KeypointId last_keypoint_id;
|
||||
|
||||
VioConfig config;
|
||||
basalt::Calibration<Scalar> calib;
|
||||
|
||||
OpticalFlowResult::Ptr transforms;
|
||||
std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
|
||||
pyramid;
|
||||
|
||||
Matrix4 E;
|
||||
|
||||
std::shared_ptr<std::thread> processing_thread;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
@@ -0,0 +1,468 @@
|
||||
/**
|
||||
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.
|
||||
*/
|
||||
|
||||
// Original source for multi-scale implementation:
|
||||
// https://github.com/DLR-RM/granite (MIT license)
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <thread>
|
||||
|
||||
#include <sophus/se2.hpp>
|
||||
|
||||
#include <tbb/blocked_range.h>
|
||||
#include <tbb/concurrent_unordered_map.h>
|
||||
#include <tbb/parallel_for.h>
|
||||
|
||||
#include <basalt/optical_flow/optical_flow.h>
|
||||
#include <basalt/optical_flow/patch.h>
|
||||
|
||||
#include <basalt/image/image_pyr.h>
|
||||
#include <basalt/utils/keypoints.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
/// MultiscaleFrameToFrameOpticalFlow is the same as FrameToFrameOpticalFlow,
|
||||
/// but patches can be created at all pyramid levels, not just the lowest
|
||||
/// pyramid.
|
||||
template <typename Scalar, template <typename> typename Pattern>
|
||||
class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
|
||||
public:
|
||||
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
|
||||
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
|
||||
|
||||
typedef Sophus::SE2<Scalar> SE2;
|
||||
|
||||
MultiscaleFrameToFrameOpticalFlow(const VioConfig& config,
|
||||
const basalt::Calibration<double>& calib)
|
||||
: t_ns(-1), frame_counter(0), last_keypoint_id(0), config(config) {
|
||||
input_queue.set_capacity(10);
|
||||
|
||||
this->calib = calib.cast<Scalar>();
|
||||
|
||||
patch_coord = PatchT::pattern2.template cast<float>();
|
||||
|
||||
if (calib.intrinsics.size() > 1) {
|
||||
Eigen::Matrix4d Ed;
|
||||
Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
|
||||
computeEssential(T_i_j, Ed);
|
||||
E = Ed.cast<Scalar>();
|
||||
}
|
||||
|
||||
processing_thread.reset(new std::thread(
|
||||
&MultiscaleFrameToFrameOpticalFlow::processingLoop, this));
|
||||
}
|
||||
|
||||
~MultiscaleFrameToFrameOpticalFlow() { processing_thread->join(); }
|
||||
|
||||
void processingLoop() {
|
||||
OpticalFlowInput::Ptr input_ptr;
|
||||
|
||||
while (true) {
|
||||
input_queue.pop(input_ptr);
|
||||
|
||||
if (!input_ptr.get()) {
|
||||
if (output_queue) output_queue->push(nullptr);
|
||||
break;
|
||||
}
|
||||
|
||||
processFrame(input_ptr->t_ns, input_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
bool processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
|
||||
for (const auto& v : new_img_vec->img_data) {
|
||||
if (!v.img.get()) {
|
||||
std::cout << "Image for " << curr_t_ns << " not present!" << std::endl;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
if (t_ns < 0) {
|
||||
t_ns = curr_t_ns;
|
||||
|
||||
transforms.reset(new OpticalFlowResult);
|
||||
transforms->observations.resize(calib.intrinsics.size());
|
||||
transforms->pyramid_levels.resize(calib.intrinsics.size());
|
||||
transforms->t_ns = t_ns;
|
||||
|
||||
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
|
||||
pyramid->resize(calib.intrinsics.size());
|
||||
|
||||
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
|
||||
[&](const tbb::blocked_range<size_t>& r) {
|
||||
for (size_t i = r.begin(); i != r.end(); ++i) {
|
||||
pyramid->at(i).setFromImage(
|
||||
*new_img_vec->img_data[i].img,
|
||||
config.optical_flow_levels);
|
||||
}
|
||||
});
|
||||
|
||||
transforms->input_images = new_img_vec;
|
||||
|
||||
addPoints();
|
||||
filterPoints();
|
||||
} else {
|
||||
t_ns = curr_t_ns;
|
||||
|
||||
old_pyramid = pyramid;
|
||||
|
||||
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
|
||||
pyramid->resize(calib.intrinsics.size());
|
||||
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
|
||||
[&](const tbb::blocked_range<size_t>& r) {
|
||||
for (size_t i = r.begin(); i != r.end(); ++i) {
|
||||
pyramid->at(i).setFromImage(
|
||||
*new_img_vec->img_data[i].img,
|
||||
config.optical_flow_levels);
|
||||
}
|
||||
});
|
||||
|
||||
OpticalFlowResult::Ptr new_transforms;
|
||||
new_transforms.reset(new OpticalFlowResult);
|
||||
new_transforms->observations.resize(calib.intrinsics.size());
|
||||
new_transforms->pyramid_levels.resize(calib.intrinsics.size());
|
||||
new_transforms->t_ns = t_ns;
|
||||
|
||||
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
|
||||
trackPoints(old_pyramid->at(i), pyramid->at(i),
|
||||
transforms->observations[i], transforms->pyramid_levels[i],
|
||||
new_transforms->observations[i],
|
||||
new_transforms->pyramid_levels[i]);
|
||||
}
|
||||
|
||||
// std::cout << t_ns << ": Could track "
|
||||
// << new_transforms->observations.at(0).size() << " points."
|
||||
// << std::endl;
|
||||
|
||||
transforms = new_transforms;
|
||||
transforms->input_images = new_img_vec;
|
||||
|
||||
addPoints();
|
||||
filterPoints();
|
||||
}
|
||||
|
||||
if (frame_counter % config.optical_flow_skip_frames == 0) {
|
||||
try {
|
||||
output_queue->push(transforms);
|
||||
} catch (const tbb::user_abort&) {
|
||||
return false;
|
||||
};
|
||||
}
|
||||
|
||||
frame_counter++;
|
||||
return true;
|
||||
}
|
||||
|
||||
void trackPoints(
|
||||
const basalt::ManagedImagePyr<uint16_t>& pyr_1,
|
||||
const basalt::ManagedImagePyr<uint16_t>& pyr_2,
|
||||
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
|
||||
transform_map_1,
|
||||
const std::map<KeypointId, size_t>& pyramid_levels_1,
|
||||
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
|
||||
std::map<KeypointId, size_t>& pyramid_levels_2) const {
|
||||
size_t num_points = transform_map_1.size();
|
||||
|
||||
std::vector<KeypointId> ids;
|
||||
Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
|
||||
std::vector<size_t> pyramid_level;
|
||||
|
||||
ids.reserve(num_points);
|
||||
init_vec.reserve(num_points);
|
||||
pyramid_level.reserve(num_points);
|
||||
|
||||
for (const auto& kv : transform_map_1) {
|
||||
ids.push_back(kv.first);
|
||||
init_vec.push_back(kv.second);
|
||||
pyramid_level.push_back(pyramid_levels_1.at(kv.first));
|
||||
}
|
||||
|
||||
tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
|
||||
std::hash<KeypointId>>
|
||||
result_transforms;
|
||||
tbb::concurrent_unordered_map<KeypointId, size_t, std::hash<KeypointId>>
|
||||
result_pyramid_level;
|
||||
|
||||
auto compute_func = [&](const tbb::blocked_range<size_t>& range) {
|
||||
for (size_t r = range.begin(); r != range.end(); ++r) {
|
||||
const KeypointId id = ids[r];
|
||||
|
||||
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
|
||||
Eigen::AffineCompact2f transform_2 = transform_1;
|
||||
|
||||
bool valid = trackPoint(pyr_1, pyr_2, transform_1, pyramid_level[r],
|
||||
transform_2);
|
||||
|
||||
if (valid) {
|
||||
Eigen::AffineCompact2f transform_1_recovered = transform_2;
|
||||
|
||||
valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r],
|
||||
transform_1_recovered);
|
||||
|
||||
if (valid) {
|
||||
const Scalar scale = 1 << pyramid_level[r];
|
||||
Scalar dist2 = (transform_1.translation() / scale -
|
||||
transform_1_recovered.translation() / scale)
|
||||
.squaredNorm();
|
||||
|
||||
if (dist2 < config.optical_flow_max_recovered_dist2) {
|
||||
result_transforms[id] = transform_2;
|
||||
result_pyramid_level[id] = pyramid_level[r];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
tbb::blocked_range<size_t> range(0, num_points);
|
||||
|
||||
tbb::parallel_for(range, compute_func);
|
||||
// compute_func(range);
|
||||
|
||||
transform_map_2.clear();
|
||||
transform_map_2.insert(result_transforms.begin(), result_transforms.end());
|
||||
pyramid_levels_2.clear();
|
||||
pyramid_levels_2.insert(result_pyramid_level.begin(),
|
||||
result_pyramid_level.end());
|
||||
}
|
||||
|
||||
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& old_pyr,
|
||||
const basalt::ManagedImagePyr<uint16_t>& pyr,
|
||||
const Eigen::AffineCompact2f& old_transform,
|
||||
const size_t pyramid_level,
|
||||
Eigen::AffineCompact2f& transform) const {
|
||||
bool patch_valid = true;
|
||||
|
||||
transform.linear().setIdentity();
|
||||
|
||||
for (ssize_t level = config.optical_flow_levels;
|
||||
level >= static_cast<ssize_t>(pyramid_level); level--) {
|
||||
const Scalar scale = 1 << level;
|
||||
|
||||
Eigen::AffineCompact2f transform_tmp = transform;
|
||||
|
||||
transform_tmp.translation() /= scale;
|
||||
|
||||
PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
|
||||
|
||||
patch_valid &= p.valid;
|
||||
if (patch_valid) {
|
||||
// Perform tracking on current level
|
||||
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform_tmp);
|
||||
}
|
||||
|
||||
if (level == static_cast<ssize_t>(pyramid_level) + 1 && !patch_valid) {
|
||||
return false;
|
||||
}
|
||||
|
||||
transform_tmp.translation() *= scale;
|
||||
|
||||
if (patch_valid) {
|
||||
transform = transform_tmp;
|
||||
}
|
||||
}
|
||||
|
||||
transform.linear() = old_transform.linear() * transform.linear();
|
||||
|
||||
return patch_valid;
|
||||
}
|
||||
|
||||
inline bool trackPointAtLevel(const Image<const uint16_t>& img_2,
|
||||
const PatchT& dp,
|
||||
Eigen::AffineCompact2f& transform) const {
|
||||
bool patch_valid = true;
|
||||
|
||||
for (int iteration = 0;
|
||||
patch_valid && iteration < config.optical_flow_max_iterations;
|
||||
iteration++) {
|
||||
typename PatchT::VectorP res;
|
||||
|
||||
typename PatchT::Matrix2P transformed_pat =
|
||||
transform.linear().matrix() * PatchT::pattern2;
|
||||
transformed_pat.colwise() += transform.translation();
|
||||
|
||||
patch_valid &= dp.residual(img_2, transformed_pat, res);
|
||||
|
||||
if (patch_valid) {
|
||||
const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
|
||||
|
||||
// avoid NaN in increment (leads to SE2::exp crashing)
|
||||
patch_valid &= inc.array().isFinite().all();
|
||||
|
||||
// avoid very large increment
|
||||
patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
|
||||
|
||||
if (patch_valid) {
|
||||
transform *= SE2::exp(inc).matrix();
|
||||
|
||||
const int filter_margin = 2;
|
||||
|
||||
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return patch_valid;
|
||||
}
|
||||
|
||||
void addPoints() {
|
||||
KeypointsData kd;
|
||||
|
||||
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses_main,
|
||||
new_poses_stereo;
|
||||
std::map<KeypointId, size_t> new_pyramid_levels_main,
|
||||
new_pyramid_levels_stereo;
|
||||
|
||||
for (ssize_t level = 0;
|
||||
level < static_cast<ssize_t>(config.optical_flow_levels) - 1;
|
||||
level++) {
|
||||
Eigen::aligned_vector<Eigen::Vector2d> pts;
|
||||
|
||||
for (const auto& kv : transforms->observations.at(0)) {
|
||||
const ssize_t point_level =
|
||||
transforms->pyramid_levels.at(0).at(kv.first);
|
||||
|
||||
// do not create points were already points at similar levels are
|
||||
if (point_level <= level + 1 && point_level >= level - 1) {
|
||||
// if (point_level == level) {
|
||||
const Scalar scale = 1 << point_level;
|
||||
pts.emplace_back(
|
||||
(kv.second.translation() / scale).template cast<double>());
|
||||
}
|
||||
}
|
||||
|
||||
detectKeypoints(pyramid->at(0).lvl(level), kd,
|
||||
config.optical_flow_detection_grid_size, 1, pts);
|
||||
|
||||
const Scalar scale = 1 << level;
|
||||
|
||||
for (size_t i = 0; i < kd.corners.size(); i++) {
|
||||
Eigen::AffineCompact2f transform;
|
||||
transform.setIdentity();
|
||||
transform.translation() =
|
||||
kd.corners[i].cast<Scalar>() * scale; // TODO cast float?
|
||||
|
||||
transforms->observations.at(0)[last_keypoint_id] = transform;
|
||||
transforms->pyramid_levels.at(0)[last_keypoint_id] = level;
|
||||
new_poses_main[last_keypoint_id] = transform;
|
||||
new_pyramid_levels_main[last_keypoint_id] = level;
|
||||
|
||||
last_keypoint_id++;
|
||||
}
|
||||
|
||||
trackPoints(pyramid->at(0), pyramid->at(1), new_poses_main,
|
||||
new_pyramid_levels_main, new_poses_stereo,
|
||||
new_pyramid_levels_stereo);
|
||||
|
||||
for (const auto& kv : new_poses_stereo) {
|
||||
transforms->observations.at(1).emplace(kv);
|
||||
transforms->pyramid_levels.at(1)[kv.first] =
|
||||
new_pyramid_levels_stereo.at(kv.first);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void filterPoints() {
|
||||
std::set<KeypointId> lm_to_remove;
|
||||
|
||||
std::vector<KeypointId> kpid;
|
||||
Eigen::aligned_vector<Eigen::Vector2f> proj0, proj1;
|
||||
|
||||
for (const auto& kv : transforms->observations.at(1)) {
|
||||
auto it = transforms->observations.at(0).find(kv.first);
|
||||
|
||||
if (it != transforms->observations.at(0).end()) {
|
||||
proj0.emplace_back(it->second.translation());
|
||||
proj1.emplace_back(kv.second.translation());
|
||||
kpid.emplace_back(kv.first);
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector4f> p3d_main, p3d_stereo;
|
||||
std::vector<bool> p3d_main_success, p3d_stereo_success;
|
||||
|
||||
calib.intrinsics[0].unproject(proj0, p3d_main, p3d_main_success);
|
||||
calib.intrinsics[1].unproject(proj1, p3d_stereo, p3d_stereo_success);
|
||||
|
||||
for (size_t i = 0; i < p3d_main_success.size(); i++) {
|
||||
if (p3d_main_success[i] && p3d_stereo_success[i]) {
|
||||
const double epipolar_error =
|
||||
std::abs(p3d_main[i].transpose() * E * p3d_stereo[i]);
|
||||
|
||||
const Scalar scale = 1 << transforms->pyramid_levels.at(0).at(kpid[i]);
|
||||
|
||||
if (epipolar_error > config.optical_flow_epipolar_error * scale) {
|
||||
lm_to_remove.emplace(kpid[i]);
|
||||
}
|
||||
} else {
|
||||
lm_to_remove.emplace(kpid[i]);
|
||||
}
|
||||
}
|
||||
|
||||
for (int id : lm_to_remove) {
|
||||
transforms->observations.at(1).erase(id);
|
||||
}
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
int64_t t_ns;
|
||||
|
||||
size_t frame_counter;
|
||||
|
||||
KeypointId last_keypoint_id;
|
||||
|
||||
VioConfig config;
|
||||
basalt::Calibration<Scalar> calib;
|
||||
|
||||
OpticalFlowResult::Ptr transforms;
|
||||
std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
|
||||
pyramid;
|
||||
|
||||
// map from stereo pair -> essential matrix
|
||||
Matrix4 E;
|
||||
|
||||
std::shared_ptr<std::thread> processing_thread;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
91
include/basalt/optical_flow/optical_flow.h
Normal file
91
include/basalt/optical_flow/optical_flow.h
Normal file
@@ -0,0 +1,91 @@
|
||||
/**
|
||||
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 <Eigen/Geometry>
|
||||
|
||||
#include <basalt/utils/vio_config.h>
|
||||
|
||||
#include <basalt/io/dataset_io.h>
|
||||
#include <basalt/calibration/calibration.hpp>
|
||||
#include <basalt/camera/stereographic_param.hpp>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
#include <tbb/concurrent_queue.h>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
using KeypointId = size_t;
|
||||
|
||||
struct OpticalFlowInput {
|
||||
using Ptr = std::shared_ptr<OpticalFlowInput>;
|
||||
|
||||
int64_t t_ns;
|
||||
std::vector<ImageData> img_data;
|
||||
std::vector<cv::Mat> img_cv_data;
|
||||
};
|
||||
|
||||
struct OpticalFlowResult {
|
||||
using Ptr = std::shared_ptr<OpticalFlowResult>;
|
||||
|
||||
int64_t t_ns;
|
||||
std::vector<Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>>
|
||||
observations;
|
||||
|
||||
std::vector<std::map<KeypointId, size_t>> pyramid_levels;
|
||||
|
||||
OpticalFlowInput::Ptr input_images;
|
||||
};
|
||||
|
||||
class OpticalFlowBase {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<OpticalFlowBase>;
|
||||
|
||||
tbb::concurrent_bounded_queue<OpticalFlowInput::Ptr> input_queue;
|
||||
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr>* output_queue = nullptr;
|
||||
|
||||
Eigen::MatrixXf patch_coord;
|
||||
};
|
||||
|
||||
class OpticalFlowFactory {
|
||||
public:
|
||||
static OpticalFlowBase::Ptr getOpticalFlow(const VioConfig& config,
|
||||
const Calibration<double>& cam);
|
||||
};
|
||||
} // namespace basalt
|
||||
226
include/basalt/optical_flow/patch.h
Normal file
226
include/basalt/optical_flow/patch.h
Normal file
@@ -0,0 +1,226 @@
|
||||
/**
|
||||
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 <sophus/se2.hpp>
|
||||
|
||||
#include <basalt/image/image.h>
|
||||
#include <basalt/optical_flow/patterns.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <typename Scalar, typename Pattern>
|
||||
struct OpticalFlowPatch {
|
||||
static constexpr int PATTERN_SIZE = Pattern::PATTERN_SIZE;
|
||||
|
||||
typedef Eigen::Matrix<int, 2, 1> Vector2i;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 1, 2> Vector2T;
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
|
||||
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
|
||||
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 1> VectorP;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
|
||||
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 2> MatrixP2;
|
||||
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 3> MatrixP3;
|
||||
typedef Eigen::Matrix<Scalar, 3, PATTERN_SIZE> Matrix3P;
|
||||
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 4> MatrixP4;
|
||||
typedef Eigen::Matrix<int, 2, PATTERN_SIZE> Matrix2Pi;
|
||||
|
||||
static const Matrix2P pattern2;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
OpticalFlowPatch() = default;
|
||||
|
||||
OpticalFlowPatch(const Image<const uint16_t> &img, const Vector2 &pos) {
|
||||
setFromImage(img, pos);
|
||||
}
|
||||
|
||||
template <typename ImgT>
|
||||
static void setData(const ImgT &img, const Vector2 &pos, Scalar &mean,
|
||||
VectorP &data, const Sophus::SE2<Scalar> *se2 = nullptr) {
|
||||
int num_valid_points = 0;
|
||||
Scalar sum = 0;
|
||||
|
||||
for (int i = 0; i < PATTERN_SIZE; i++) {
|
||||
Vector2 p;
|
||||
if (se2) {
|
||||
p = pos + (*se2) * pattern2.col(i);
|
||||
} else {
|
||||
p = pos + pattern2.col(i);
|
||||
};
|
||||
|
||||
if (img.InBounds(p, 2)) {
|
||||
Scalar val = img.template interp<Scalar>(p);
|
||||
data[i] = val;
|
||||
sum += val;
|
||||
num_valid_points++;
|
||||
} else {
|
||||
data[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
mean = sum / num_valid_points;
|
||||
data /= mean;
|
||||
}
|
||||
|
||||
template <typename ImgT>
|
||||
static void setDataJacSe2(const ImgT &img, const Vector2 &pos, Scalar &mean,
|
||||
VectorP &data, MatrixP3 &J_se2) {
|
||||
int num_valid_points = 0;
|
||||
Scalar sum = 0;
|
||||
Vector3 grad_sum_se2(0, 0, 0);
|
||||
|
||||
Eigen::Matrix<Scalar, 2, 3> Jw_se2;
|
||||
Jw_se2.template topLeftCorner<2, 2>().setIdentity();
|
||||
|
||||
for (int i = 0; i < PATTERN_SIZE; i++) {
|
||||
Vector2 p = pos + pattern2.col(i);
|
||||
|
||||
// Fill jacobians with respect to SE2 warp
|
||||
Jw_se2(0, 2) = -pattern2(1, i);
|
||||
Jw_se2(1, 2) = pattern2(0, i);
|
||||
|
||||
if (img.InBounds(p, 2)) {
|
||||
Vector3 valGrad = img.template interpGrad<Scalar>(p);
|
||||
data[i] = valGrad[0];
|
||||
sum += valGrad[0];
|
||||
J_se2.row(i) = valGrad.template tail<2>().transpose() * Jw_se2;
|
||||
grad_sum_se2 += J_se2.row(i);
|
||||
num_valid_points++;
|
||||
} else {
|
||||
data[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
mean = sum / num_valid_points;
|
||||
|
||||
const Scalar mean_inv = num_valid_points / sum;
|
||||
|
||||
for (int i = 0; i < PATTERN_SIZE; i++) {
|
||||
if (data[i] >= 0) {
|
||||
J_se2.row(i) -= grad_sum_se2.transpose() * data[i] / sum;
|
||||
data[i] *= mean_inv;
|
||||
} else {
|
||||
J_se2.row(i).setZero();
|
||||
}
|
||||
}
|
||||
J_se2 *= mean_inv;
|
||||
}
|
||||
|
||||
void setFromImage(const Image<const uint16_t> &img, const Vector2 &pos) {
|
||||
this->pos = pos;
|
||||
|
||||
MatrixP3 J_se2;
|
||||
|
||||
setDataJacSe2(img, pos, mean, data, J_se2);
|
||||
|
||||
Matrix3 H_se2 = J_se2.transpose() * J_se2;
|
||||
Matrix3 H_se2_inv;
|
||||
H_se2_inv.setIdentity();
|
||||
H_se2.ldlt().solveInPlace(H_se2_inv);
|
||||
|
||||
H_se2_inv_J_se2_T = H_se2_inv * J_se2.transpose();
|
||||
|
||||
// NOTE: while it's very unlikely we get a source patch with all black
|
||||
// pixels, since points are usually selected at corners, it doesn't cost
|
||||
// much to be safe here.
|
||||
|
||||
// all-black patch cannot be normalized; will result in mean of "zero" and
|
||||
// H_se2_inv_J_se2_T will contain "NaN" and data will contain "inf"
|
||||
valid = mean > std::numeric_limits<Scalar>::epsilon() &&
|
||||
H_se2_inv_J_se2_T.array().isFinite().all() &&
|
||||
data.array().isFinite().all();
|
||||
}
|
||||
|
||||
inline bool residual(const Image<const uint16_t> &img,
|
||||
const Matrix2P &transformed_pattern,
|
||||
VectorP &residual) const {
|
||||
Scalar sum = 0;
|
||||
int num_valid_points = 0;
|
||||
|
||||
for (int i = 0; i < PATTERN_SIZE; i++) {
|
||||
if (img.InBounds(transformed_pattern.col(i), 2)) {
|
||||
residual[i] = img.interp<Scalar>(transformed_pattern.col(i));
|
||||
sum += residual[i];
|
||||
num_valid_points++;
|
||||
} else {
|
||||
residual[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// all-black patch cannot be normalized
|
||||
if (sum < std::numeric_limits<Scalar>::epsilon()) {
|
||||
residual.setZero();
|
||||
return false;
|
||||
}
|
||||
|
||||
int num_residuals = 0;
|
||||
|
||||
for (int i = 0; i < PATTERN_SIZE; i++) {
|
||||
if (residual[i] >= 0 && data[i] >= 0) {
|
||||
const Scalar val = residual[i];
|
||||
residual[i] = num_valid_points * val / sum - data[i];
|
||||
num_residuals++;
|
||||
|
||||
} else {
|
||||
residual[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return num_residuals > PATTERN_SIZE / 2;
|
||||
}
|
||||
|
||||
Vector2 pos = Vector2::Zero();
|
||||
VectorP data = VectorP::Zero(); // negative if the point is not valid
|
||||
|
||||
// MatrixP3 J_se2; // total jacobian with respect to se2 warp
|
||||
// Matrix3 H_se2_inv;
|
||||
Matrix3P H_se2_inv_J_se2_T = Matrix3P::Zero();
|
||||
|
||||
Scalar mean = 0;
|
||||
|
||||
bool valid = false;
|
||||
};
|
||||
|
||||
template <typename Scalar, typename Pattern>
|
||||
const typename OpticalFlowPatch<Scalar, Pattern>::Matrix2P
|
||||
OpticalFlowPatch<Scalar, Pattern>::pattern2 = Pattern::pattern2;
|
||||
|
||||
} // namespace basalt
|
||||
410
include/basalt/optical_flow/patch_optical_flow.h
Normal file
410
include/basalt/optical_flow/patch_optical_flow.h
Normal file
@@ -0,0 +1,410 @@
|
||||
/**
|
||||
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 <sophus/se2.hpp>
|
||||
|
||||
#include <tbb/blocked_range.h>
|
||||
#include <tbb/concurrent_unordered_map.h>
|
||||
#include <tbb/parallel_for.h>
|
||||
|
||||
#include <basalt/optical_flow/optical_flow.h>
|
||||
#include <basalt/optical_flow/patch.h>
|
||||
|
||||
#include <basalt/image/image_pyr.h>
|
||||
#include <basalt/utils/keypoints.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
// TODO: patches are currently never erased, so over time memory consumption
|
||||
// increases
|
||||
// TODO: some changes from FrameToFrameOpticalFlow could be back-ported
|
||||
// (adjustments to Scalar=float, tbb parallelization, ...).
|
||||
|
||||
/// PatchOpticalFlow keeps reference patches from the frame where the point was
|
||||
/// initially created. Should result in more consistent tracks (less drift over
|
||||
/// time) than frame-to-frame tracking, but it results in shorter tracks in
|
||||
/// practice.
|
||||
template <typename Scalar, template <typename> typename Pattern>
|
||||
class PatchOpticalFlow : public OpticalFlowBase {
|
||||
public:
|
||||
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
|
||||
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
|
||||
|
||||
typedef Sophus::SE2<Scalar> SE2;
|
||||
|
||||
PatchOpticalFlow(const VioConfig& config,
|
||||
const basalt::Calibration<double>& calib)
|
||||
: t_ns(-1),
|
||||
frame_counter(0),
|
||||
last_keypoint_id(0),
|
||||
config(config),
|
||||
calib(calib) {
|
||||
patches.reserve(3000);
|
||||
input_queue.set_capacity(10);
|
||||
|
||||
patch_coord = PatchT::pattern2.template cast<float>();
|
||||
|
||||
if (calib.intrinsics.size() > 1) {
|
||||
Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
|
||||
computeEssential(T_i_j, E);
|
||||
}
|
||||
|
||||
processing_thread.reset(
|
||||
new std::thread(&PatchOpticalFlow::processingLoop, this));
|
||||
}
|
||||
|
||||
~PatchOpticalFlow() { processing_thread->join(); }
|
||||
|
||||
void processingLoop() {
|
||||
OpticalFlowInput::Ptr input_ptr;
|
||||
|
||||
while (true) {
|
||||
input_queue.pop(input_ptr);
|
||||
|
||||
if (!input_ptr.get()) {
|
||||
output_queue->push(nullptr);
|
||||
break;
|
||||
}
|
||||
|
||||
processFrame(input_ptr->t_ns, input_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
void processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
|
||||
for (const auto& v : new_img_vec->img_data) {
|
||||
if (!v.img.get()) return;
|
||||
}
|
||||
|
||||
if (t_ns < 0) {
|
||||
t_ns = curr_t_ns;
|
||||
|
||||
transforms.reset(new OpticalFlowResult);
|
||||
transforms->observations.resize(calib.intrinsics.size());
|
||||
transforms->t_ns = t_ns;
|
||||
|
||||
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
|
||||
pyramid->resize(calib.intrinsics.size());
|
||||
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
|
||||
pyramid->at(i).setFromImage(*new_img_vec->img_data[i].img,
|
||||
config.optical_flow_levels);
|
||||
}
|
||||
|
||||
transforms->input_images = new_img_vec;
|
||||
|
||||
addPoints();
|
||||
filterPoints();
|
||||
|
||||
} else {
|
||||
t_ns = curr_t_ns;
|
||||
|
||||
old_pyramid = pyramid;
|
||||
|
||||
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
|
||||
pyramid->resize(calib.intrinsics.size());
|
||||
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
|
||||
pyramid->at(i).setFromImage(*new_img_vec->img_data[i].img,
|
||||
config.optical_flow_levels);
|
||||
}
|
||||
|
||||
OpticalFlowResult::Ptr new_transforms;
|
||||
new_transforms.reset(new OpticalFlowResult);
|
||||
new_transforms->observations.resize(new_img_vec->img_data.size());
|
||||
new_transforms->t_ns = t_ns;
|
||||
|
||||
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
|
||||
trackPoints(old_pyramid->at(i), pyramid->at(i),
|
||||
transforms->observations[i],
|
||||
new_transforms->observations[i]);
|
||||
}
|
||||
|
||||
transforms = new_transforms;
|
||||
transforms->input_images = new_img_vec;
|
||||
|
||||
addPoints();
|
||||
filterPoints();
|
||||
}
|
||||
|
||||
if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) {
|
||||
output_queue->push(transforms);
|
||||
}
|
||||
frame_counter++;
|
||||
}
|
||||
|
||||
void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1,
|
||||
const basalt::ManagedImagePyr<uint16_t>& pyr_2,
|
||||
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
|
||||
transform_map_1,
|
||||
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
|
||||
transform_map_2) const {
|
||||
size_t num_points = transform_map_1.size();
|
||||
|
||||
std::vector<KeypointId> ids;
|
||||
Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
|
||||
|
||||
ids.reserve(num_points);
|
||||
init_vec.reserve(num_points);
|
||||
|
||||
for (const auto& kv : transform_map_1) {
|
||||
ids.push_back(kv.first);
|
||||
init_vec.push_back(kv.second);
|
||||
}
|
||||
|
||||
tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
|
||||
std::hash<KeypointId>>
|
||||
result;
|
||||
|
||||
auto compute_func = [&](const tbb::blocked_range<size_t>& range) {
|
||||
for (size_t r = range.begin(); r != range.end(); ++r) {
|
||||
const KeypointId id = ids[r];
|
||||
|
||||
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
|
||||
Eigen::AffineCompact2f transform_2 = transform_1;
|
||||
|
||||
const Eigen::aligned_vector<PatchT>& patch_vec = patches.at(id);
|
||||
|
||||
bool valid = trackPoint(pyr_2, patch_vec, transform_2);
|
||||
|
||||
if (valid) {
|
||||
Eigen::AffineCompact2f transform_1_recovered = transform_2;
|
||||
|
||||
valid = trackPoint(pyr_1, patch_vec, transform_1_recovered);
|
||||
|
||||
if (valid) {
|
||||
Scalar dist2 = (transform_1.translation() -
|
||||
transform_1_recovered.translation())
|
||||
.squaredNorm();
|
||||
|
||||
if (dist2 < config.optical_flow_max_recovered_dist2) {
|
||||
result[id] = transform_2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
tbb::blocked_range<size_t> range(0, num_points);
|
||||
|
||||
tbb::parallel_for(range, compute_func);
|
||||
// compute_func(range);
|
||||
|
||||
transform_map_2.clear();
|
||||
transform_map_2.insert(result.begin(), result.end());
|
||||
}
|
||||
|
||||
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& pyr,
|
||||
const Eigen::aligned_vector<PatchT>& patch_vec,
|
||||
Eigen::AffineCompact2f& transform) const {
|
||||
bool patch_valid = true;
|
||||
|
||||
for (int level = config.optical_flow_levels; level >= 0 && patch_valid;
|
||||
level--) {
|
||||
const Scalar scale = 1 << level;
|
||||
|
||||
transform.translation() /= scale;
|
||||
|
||||
// TODO: maybe we should better check patch validity when creating points
|
||||
const auto& p = patch_vec[level];
|
||||
patch_valid &= p.valid;
|
||||
if (patch_valid) {
|
||||
// Perform tracking on current level
|
||||
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform);
|
||||
}
|
||||
|
||||
transform.translation() *= scale;
|
||||
}
|
||||
|
||||
return patch_valid;
|
||||
}
|
||||
|
||||
inline bool trackPointAtLevel(const Image<const uint16_t>& img_2,
|
||||
const PatchT& dp,
|
||||
Eigen::AffineCompact2f& transform) const {
|
||||
bool patch_valid = true;
|
||||
|
||||
for (int iteration = 0;
|
||||
patch_valid && iteration < config.optical_flow_max_iterations;
|
||||
iteration++) {
|
||||
typename PatchT::VectorP res;
|
||||
|
||||
typename PatchT::Matrix2P transformed_pat =
|
||||
transform.linear().matrix() * PatchT::pattern2;
|
||||
transformed_pat.colwise() += transform.translation();
|
||||
|
||||
patch_valid &= dp.residual(img_2, transformed_pat, res);
|
||||
|
||||
if (patch_valid) {
|
||||
const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
|
||||
|
||||
// avoid NaN in increment (leads to SE2::exp crashing)
|
||||
patch_valid &= inc.array().isFinite().all();
|
||||
|
||||
// avoid very large increment
|
||||
patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
|
||||
|
||||
if (patch_valid) {
|
||||
transform *= SE2::exp(inc).matrix();
|
||||
|
||||
const int filter_margin = 2;
|
||||
|
||||
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return patch_valid;
|
||||
}
|
||||
|
||||
void addPoints() {
|
||||
Eigen::aligned_vector<Eigen::Vector2d> pts0;
|
||||
|
||||
for (const auto& kv : transforms->observations.at(0)) {
|
||||
pts0.emplace_back(kv.second.translation().cast<double>());
|
||||
}
|
||||
|
||||
KeypointsData kd;
|
||||
|
||||
detectKeypoints(pyramid->at(0).lvl(0), kd,
|
||||
config.optical_flow_detection_grid_size, 1, pts0);
|
||||
|
||||
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses0,
|
||||
new_poses1;
|
||||
|
||||
for (size_t i = 0; i < kd.corners.size(); i++) {
|
||||
Eigen::aligned_vector<PatchT>& p = patches[last_keypoint_id];
|
||||
|
||||
Vector2 pos = kd.corners[i].cast<Scalar>();
|
||||
|
||||
for (int l = 0; l <= config.optical_flow_levels; l++) {
|
||||
Scalar scale = 1 << l;
|
||||
Vector2 pos_scaled = pos / scale;
|
||||
p.emplace_back(pyramid->at(0).lvl(l), pos_scaled);
|
||||
}
|
||||
|
||||
Eigen::AffineCompact2f transform;
|
||||
transform.setIdentity();
|
||||
transform.translation() = kd.corners[i].cast<Scalar>();
|
||||
|
||||
transforms->observations.at(0)[last_keypoint_id] = transform;
|
||||
new_poses0[last_keypoint_id] = transform;
|
||||
|
||||
last_keypoint_id++;
|
||||
}
|
||||
|
||||
if (calib.intrinsics.size() > 1) {
|
||||
trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1);
|
||||
|
||||
for (const auto& kv : new_poses1) {
|
||||
transforms->observations.at(1).emplace(kv);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void filterPoints() {
|
||||
if (calib.intrinsics.size() < 2) return;
|
||||
|
||||
std::set<KeypointId> lm_to_remove;
|
||||
|
||||
std::vector<KeypointId> kpid;
|
||||
Eigen::aligned_vector<Eigen::Vector2d> proj0, proj1;
|
||||
|
||||
for (const auto& kv : transforms->observations.at(1)) {
|
||||
auto it = transforms->observations.at(0).find(kv.first);
|
||||
|
||||
if (it != transforms->observations.at(0).end()) {
|
||||
proj0.emplace_back(it->second.translation().cast<double>());
|
||||
proj1.emplace_back(kv.second.translation().cast<double>());
|
||||
kpid.emplace_back(kv.first);
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector4d> p3d0, p3d1;
|
||||
std::vector<bool> p3d0_success, p3d1_success;
|
||||
|
||||
calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success);
|
||||
calib.intrinsics[1].unproject(proj1, p3d1, p3d1_success);
|
||||
|
||||
for (size_t i = 0; i < p3d0_success.size(); i++) {
|
||||
if (p3d0_success[i] && p3d1_success[i]) {
|
||||
const double epipolar_error =
|
||||
std::abs(p3d0[i].transpose() * E * p3d1[i]);
|
||||
|
||||
if (epipolar_error > config.optical_flow_epipolar_error) {
|
||||
lm_to_remove.emplace(kpid[i]);
|
||||
}
|
||||
} else {
|
||||
lm_to_remove.emplace(kpid[i]);
|
||||
}
|
||||
}
|
||||
|
||||
for (int id : lm_to_remove) {
|
||||
transforms->observations.at(1).erase(id);
|
||||
}
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
int64_t t_ns;
|
||||
|
||||
size_t frame_counter;
|
||||
|
||||
KeypointId last_keypoint_id;
|
||||
|
||||
VioConfig config;
|
||||
basalt::Calibration<double> calib;
|
||||
|
||||
Eigen::aligned_unordered_map<KeypointId, Eigen::aligned_vector<PatchT>>
|
||||
patches;
|
||||
|
||||
OpticalFlowResult::Ptr transforms;
|
||||
std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
|
||||
pyramid;
|
||||
|
||||
Eigen::Matrix4d E;
|
||||
|
||||
std::shared_ptr<std::thread> processing_thread;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
171
include/basalt/optical_flow/patterns.h
Normal file
171
include/basalt/optical_flow/patterns.h
Normal file
@@ -0,0 +1,171 @@
|
||||
/**
|
||||
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>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Scalar>
|
||||
struct Pattern24 {
|
||||
// 00 01
|
||||
//
|
||||
// 02 03 04 05
|
||||
//
|
||||
// 06 07 08 09 10 11
|
||||
//
|
||||
// 12 13 14 15 16 17
|
||||
//
|
||||
// 18 19 20 21
|
||||
//
|
||||
// 22 23
|
||||
//
|
||||
// -----> x
|
||||
// |
|
||||
// |
|
||||
// y
|
||||
|
||||
static constexpr Scalar pattern_raw[][2] = {
|
||||
{-1, 5}, {1, 5},
|
||||
|
||||
{-3, 3}, {-1, 3}, {1, 3}, {3, 3},
|
||||
|
||||
{-5, 1}, {-3, 1}, {-1, 1}, {1, 1}, {3, 1}, {5, 1},
|
||||
|
||||
{-5, -1}, {-3, -1}, {-1, -1}, {1, -1}, {3, -1}, {5, -1},
|
||||
|
||||
{-3, -3}, {-1, -3}, {1, -3}, {3, -3},
|
||||
|
||||
{-1, -5}, {1, -5}
|
||||
|
||||
};
|
||||
|
||||
static constexpr int PATTERN_SIZE =
|
||||
sizeof(pattern_raw) / (2 * sizeof(Scalar));
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
|
||||
static const Matrix2P pattern2;
|
||||
};
|
||||
|
||||
template <class Scalar>
|
||||
const typename Pattern24<Scalar>::Matrix2P Pattern24<Scalar>::pattern2 =
|
||||
Eigen::Map<Pattern24<Scalar>::Matrix2P>((Scalar *)
|
||||
Pattern24<Scalar>::pattern_raw);
|
||||
|
||||
template <class Scalar>
|
||||
struct Pattern52 {
|
||||
// 00 01 02 03
|
||||
//
|
||||
// 04 05 06 07 08 09
|
||||
//
|
||||
// 10 11 12 13 14 15 16 17
|
||||
//
|
||||
// 18 19 20 21 22 23 24 25
|
||||
//
|
||||
// 26 27 28 29 30 31 32 33
|
||||
//
|
||||
// 34 35 36 37 38 39 40 41
|
||||
//
|
||||
// 42 43 44 45 46 47
|
||||
//
|
||||
// 48 49 50 51
|
||||
//
|
||||
// -----> x
|
||||
// |
|
||||
// |
|
||||
// y
|
||||
|
||||
static constexpr Scalar pattern_raw[][2] = {
|
||||
{-3, 7}, {-1, 7}, {1, 7}, {3, 7},
|
||||
|
||||
{-5, 5}, {-3, 5}, {-1, 5}, {1, 5}, {3, 5}, {5, 5},
|
||||
|
||||
{-7, 3}, {-5, 3}, {-3, 3}, {-1, 3}, {1, 3}, {3, 3},
|
||||
{5, 3}, {7, 3},
|
||||
|
||||
{-7, 1}, {-5, 1}, {-3, 1}, {-1, 1}, {1, 1}, {3, 1},
|
||||
{5, 1}, {7, 1},
|
||||
|
||||
{-7, -1}, {-5, -1}, {-3, -1}, {-1, -1}, {1, -1}, {3, -1},
|
||||
{5, -1}, {7, -1},
|
||||
|
||||
{-7, -3}, {-5, -3}, {-3, -3}, {-1, -3}, {1, -3}, {3, -3},
|
||||
{5, -3}, {7, -3},
|
||||
|
||||
{-5, -5}, {-3, -5}, {-1, -5}, {1, -5}, {3, -5}, {5, -5},
|
||||
|
||||
{-3, -7}, {-1, -7}, {1, -7}, {3, -7}
|
||||
|
||||
};
|
||||
|
||||
static constexpr int PATTERN_SIZE =
|
||||
sizeof(pattern_raw) / (2 * sizeof(Scalar));
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
|
||||
static const Matrix2P pattern2;
|
||||
};
|
||||
|
||||
template <class Scalar>
|
||||
const typename Pattern52<Scalar>::Matrix2P Pattern52<Scalar>::pattern2 =
|
||||
Eigen::Map<Pattern52<Scalar>::Matrix2P>((Scalar *)
|
||||
Pattern52<Scalar>::pattern_raw);
|
||||
|
||||
// Same as Pattern52 but twice smaller
|
||||
template <class Scalar>
|
||||
struct Pattern51 {
|
||||
static constexpr int PATTERN_SIZE = Pattern52<Scalar>::PATTERN_SIZE;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
|
||||
static const Matrix2P pattern2;
|
||||
};
|
||||
|
||||
template <class Scalar>
|
||||
const typename Pattern51<Scalar>::Matrix2P Pattern51<Scalar>::pattern2 =
|
||||
0.5 * Pattern52<Scalar>::pattern2;
|
||||
|
||||
// Same as Pattern52 but 0.75 smaller
|
||||
template <class Scalar>
|
||||
struct Pattern50 {
|
||||
static constexpr int PATTERN_SIZE = Pattern52<Scalar>::PATTERN_SIZE;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
|
||||
static const Matrix2P pattern2;
|
||||
};
|
||||
|
||||
template <class Scalar>
|
||||
const typename Pattern50<Scalar>::Matrix2P Pattern50<Scalar>::pattern2 =
|
||||
0.75 * Pattern52<Scalar>::pattern2;
|
||||
|
||||
} // namespace basalt
|
||||
281
include/basalt/optimization/accumulator.h
Normal file
281
include/basalt/optimization/accumulator.h
Normal file
@@ -0,0 +1,281 @@
|
||||
/**
|
||||
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 <Eigen/Sparse>
|
||||
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <unordered_map>
|
||||
|
||||
#include <basalt/utils/assert.h>
|
||||
#include <basalt/utils/hash.h>
|
||||
|
||||
#if defined(BASALT_USE_CHOLMOD)
|
||||
|
||||
#include <Eigen/CholmodSupport>
|
||||
|
||||
template <class T>
|
||||
using SparseLLT = Eigen::CholmodSupernodalLLT<T>;
|
||||
|
||||
#else
|
||||
|
||||
template <class T>
|
||||
using SparseLLT = Eigen::SimplicialLDLT<T>;
|
||||
|
||||
#endif
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <typename Scalar_ = double>
|
||||
class DenseAccumulator {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
|
||||
|
||||
template <int ROWS, int COLS, typename Derived>
|
||||
inline void addH(int i, int j, const Eigen::MatrixBase<Derived>& data) {
|
||||
BASALT_ASSERT_STREAM(i >= 0, "i " << i);
|
||||
BASALT_ASSERT_STREAM(j >= 0, "j " << j);
|
||||
|
||||
BASALT_ASSERT_STREAM(i + ROWS <= H.cols(), "i " << i << " ROWS " << ROWS
|
||||
<< " H.rows() "
|
||||
<< H.rows());
|
||||
BASALT_ASSERT_STREAM(j + COLS <= H.rows(), "j " << j << " COLS " << COLS
|
||||
<< " H.cols() "
|
||||
<< H.cols());
|
||||
|
||||
H.template block<ROWS, COLS>(i, j) += data;
|
||||
}
|
||||
|
||||
template <int ROWS, typename Derived>
|
||||
inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
|
||||
BASALT_ASSERT_STREAM(i >= 0, "i " << i);
|
||||
|
||||
BASALT_ASSERT_STREAM(i + ROWS <= H.cols(), "i " << i << " ROWS " << ROWS
|
||||
<< " H.rows() "
|
||||
<< H.rows());
|
||||
|
||||
b.template segment<ROWS>(i) += data;
|
||||
}
|
||||
|
||||
// inline VectorX solve() const { return H.ldlt().solve(b); }
|
||||
inline VectorX solve(const VectorX* diagonal) const {
|
||||
if (diagonal == nullptr) {
|
||||
return H.ldlt().solve(b);
|
||||
} else {
|
||||
MatrixX HH = H;
|
||||
HH.diagonal() += *diagonal;
|
||||
return HH.ldlt().solve(b);
|
||||
}
|
||||
}
|
||||
|
||||
inline void reset(int opt_size) {
|
||||
H.setZero(opt_size, opt_size);
|
||||
b.setZero(opt_size);
|
||||
}
|
||||
|
||||
inline void join(const DenseAccumulator<Scalar>& other) {
|
||||
H += other.H;
|
||||
b += other.b;
|
||||
}
|
||||
|
||||
inline void print() {
|
||||
Eigen::IOFormat CleanFmt(2);
|
||||
std::cerr << "H\n" << H.format(CleanFmt) << std::endl;
|
||||
std::cerr << "b\n" << b.transpose().format(CleanFmt) << std::endl;
|
||||
}
|
||||
|
||||
inline void setup_solver(){};
|
||||
inline VectorX Hdiagonal() const { return H.diagonal(); }
|
||||
|
||||
inline const MatrixX& getH() const { return H; }
|
||||
inline const VectorX& getB() const { return b; }
|
||||
|
||||
inline MatrixX& getH() { return H; }
|
||||
inline VectorX& getB() { return b; }
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
MatrixX H;
|
||||
VectorX b;
|
||||
};
|
||||
|
||||
template <typename Scalar_ = double>
|
||||
class SparseHashAccumulator {
|
||||
public:
|
||||
using Scalar = Scalar_;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
|
||||
typedef Eigen::Triplet<Scalar> T;
|
||||
typedef Eigen::SparseMatrix<Scalar> SparseMatrix;
|
||||
|
||||
template <int ROWS, int COLS, typename Derived>
|
||||
inline void addH(int si, int sj, const Eigen::MatrixBase<Derived>& data) {
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, ROWS, COLS);
|
||||
|
||||
KeyT id;
|
||||
id[0] = si;
|
||||
id[1] = sj;
|
||||
id[2] = ROWS;
|
||||
id[3] = COLS;
|
||||
|
||||
auto it = hash_map.find(id);
|
||||
|
||||
if (it == hash_map.end()) {
|
||||
hash_map.emplace(id, data);
|
||||
} else {
|
||||
it->second += data;
|
||||
}
|
||||
}
|
||||
|
||||
template <int ROWS, typename Derived>
|
||||
inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
|
||||
b.template segment<ROWS>(i) += data;
|
||||
}
|
||||
|
||||
inline void setup_solver() {
|
||||
std::vector<T> triplets;
|
||||
triplets.reserve(hash_map.size() * 36 + b.rows());
|
||||
|
||||
for (const auto& kv : hash_map) {
|
||||
for (int i = 0; i < kv.second.rows(); i++) {
|
||||
for (int j = 0; j < kv.second.cols(); j++) {
|
||||
triplets.emplace_back(kv.first[0] + i, kv.first[1] + j,
|
||||
kv.second(i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < b.rows(); i++) {
|
||||
triplets.emplace_back(i, i, std::numeric_limits<double>::min());
|
||||
}
|
||||
|
||||
smm = SparseMatrix(b.rows(), b.rows());
|
||||
smm.setFromTriplets(triplets.begin(), triplets.end());
|
||||
}
|
||||
|
||||
inline VectorX Hdiagonal() const { return smm.diagonal(); }
|
||||
|
||||
inline VectorX& getB() { return b; }
|
||||
|
||||
inline VectorX solve(const VectorX* diagonal) const {
|
||||
auto t2 = std::chrono::high_resolution_clock::now();
|
||||
|
||||
SparseMatrix sm = smm;
|
||||
if (diagonal) sm.diagonal() += *diagonal;
|
||||
|
||||
VectorX res;
|
||||
|
||||
if (iterative_solver) {
|
||||
// NOTE: since we have to disable Eigen's parallelization with OpenMP
|
||||
// (interferes with TBB), the current CG is single-threaded, and we
|
||||
// can expect a substantial speedup by switching to a parallel
|
||||
// implementation of CG.
|
||||
Eigen::ConjugateGradient<Eigen::SparseMatrix<double>,
|
||||
Eigen::Lower | Eigen::Upper>
|
||||
cg;
|
||||
|
||||
cg.setTolerance(tolerance);
|
||||
cg.compute(sm);
|
||||
res = cg.solve(b);
|
||||
} else {
|
||||
SparseLLT<SparseMatrix> chol(sm);
|
||||
res = chol.solve(b);
|
||||
}
|
||||
|
||||
auto t3 = std::chrono::high_resolution_clock::now();
|
||||
|
||||
auto elapsed2 =
|
||||
std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2);
|
||||
|
||||
if (print_info) {
|
||||
std::cout << "Solving linear system: " << elapsed2.count() * 1e-6 << "s."
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
inline void reset(int opt_size) {
|
||||
hash_map.clear();
|
||||
b.setZero(opt_size);
|
||||
}
|
||||
|
||||
inline void join(const SparseHashAccumulator<Scalar>& other) {
|
||||
for (const auto& kv : other.hash_map) {
|
||||
auto it = hash_map.find(kv.first);
|
||||
|
||||
if (it == hash_map.end()) {
|
||||
hash_map.emplace(kv.first, kv.second);
|
||||
} else {
|
||||
it->second += kv.second;
|
||||
}
|
||||
}
|
||||
|
||||
b += other.b;
|
||||
}
|
||||
|
||||
double tolerance = 1e-4;
|
||||
bool iterative_solver = false;
|
||||
bool print_info = false;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
using KeyT = std::array<int, 4>;
|
||||
|
||||
struct KeyHash {
|
||||
inline size_t operator()(const KeyT& c) const {
|
||||
size_t seed = 0;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
hash_combine(seed, c[i]);
|
||||
}
|
||||
return seed;
|
||||
}
|
||||
};
|
||||
|
||||
std::unordered_map<KeyT, MatrixX, KeyHash> hash_map;
|
||||
|
||||
VectorX b;
|
||||
|
||||
SparseMatrix smm;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
195
include/basalt/optimization/linearize.h
Normal file
195
include/basalt/optimization/linearize.h
Normal file
@@ -0,0 +1,195 @@
|
||||
/**
|
||||
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
|
||||
264
include/basalt/optimization/poses_linearize.h
Normal file
264
include/basalt/optimization/poses_linearize.h
Normal file
@@ -0,0 +1,264 @@
|
||||
/**
|
||||
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_POSES_LINEARIZE_H
|
||||
#define BASALT_POSES_LINEARIZE_H
|
||||
|
||||
#include <basalt/io/dataset_io.h>
|
||||
#include <basalt/spline/se3_spline.h>
|
||||
#include <basalt/calibration/calibration.hpp>
|
||||
|
||||
#include <basalt/optimization/linearize.h>
|
||||
|
||||
#include <basalt/optimization/accumulator.h>
|
||||
|
||||
#include <tbb/blocked_range.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <typename Scalar, typename AccumT>
|
||||
struct LinearizePosesOpt : public LinearizeBase<Scalar> {
|
||||
static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
|
||||
|
||||
typedef Sophus::SE3<Scalar> SE3;
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
|
||||
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<AprilgridCornersData>::const_iterator
|
||||
AprilgridCornersDataIter;
|
||||
|
||||
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
|
||||
|
||||
AccumT accum;
|
||||
Scalar error;
|
||||
Scalar reprojection_error;
|
||||
int num_points;
|
||||
|
||||
size_t opt_size;
|
||||
|
||||
const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
|
||||
|
||||
LinearizePosesOpt(
|
||||
size_t opt_size,
|
||||
const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose,
|
||||
const CalibCommonData& common_data)
|
||||
: opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
|
||||
this->common_data = common_data;
|
||||
accum.reset(opt_size);
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
}
|
||||
LinearizePosesOpt(const LinearizePosesOpt& other, tbb::split)
|
||||
: opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) {
|
||||
this->common_data = other.common_data;
|
||||
accum.reset(opt_size);
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
|
||||
for (const AprilgridCornersData& acd : r) {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
constexpr int INTRINSICS_SIZE =
|
||||
std::remove_reference<decltype(cam)>::type::N;
|
||||
typename LinearizeBase<Scalar>::template PoseCalibH<INTRINSICS_SIZE>
|
||||
cph;
|
||||
|
||||
SE3 T_w_i = timestam_to_pose.at(acd.timestamp_ns);
|
||||
SE3 T_w_c =
|
||||
T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
|
||||
SE3 T_c_w = T_w_c.inverse();
|
||||
Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
|
||||
|
||||
double err = 0;
|
||||
double reproj_err = 0;
|
||||
int num_inliers = 0;
|
||||
|
||||
for (size_t i = 0; i < acd.corner_pos.size(); i++) {
|
||||
this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
|
||||
T_c_w_m, cam, &cph, err, num_inliers,
|
||||
reproj_err);
|
||||
}
|
||||
|
||||
error += err;
|
||||
reprojection_error += reproj_err;
|
||||
num_points += num_inliers;
|
||||
|
||||
const Matrix6 Adj =
|
||||
-this->common_data.calibration->T_i_c[acd.cam_id]
|
||||
.inverse()
|
||||
.Adj();
|
||||
|
||||
const size_t po =
|
||||
this->common_data.offset_poses->at(acd.timestamp_ns);
|
||||
const size_t co = this->common_data.offset_T_i_c->at(acd.cam_id);
|
||||
const size_t io =
|
||||
this->common_data.offset_intrinsics->at(acd.cam_id);
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
po, po, Adj.transpose() * cph.H_pose_accum * Adj);
|
||||
accum.template addB<POSE_SIZE>(po,
|
||||
Adj.transpose() * cph.b_pose_accum);
|
||||
|
||||
if (acd.cam_id > 0) {
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
co, po, -cph.H_pose_accum * Adj);
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(co, co,
|
||||
cph.H_pose_accum);
|
||||
|
||||
accum.template addB<POSE_SIZE>(co, -cph.b_pose_accum);
|
||||
}
|
||||
|
||||
if (this->common_data.opt_intrinsics) {
|
||||
accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
|
||||
io, po, cph.H_intr_pose_accum * Adj);
|
||||
|
||||
if (acd.cam_id > 0)
|
||||
accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
|
||||
io, co, -cph.H_intr_pose_accum);
|
||||
|
||||
accum.template addH<INTRINSICS_SIZE, INTRINSICS_SIZE>(
|
||||
io, io, cph.H_intr_accum);
|
||||
accum.template addB<INTRINSICS_SIZE>(io, cph.b_intr_accum);
|
||||
}
|
||||
},
|
||||
this->common_data.calibration->intrinsics[acd.cam_id].variant);
|
||||
}
|
||||
}
|
||||
|
||||
void join(LinearizePosesOpt& rhs) {
|
||||
accum.join(rhs.accum);
|
||||
error += rhs.error;
|
||||
reprojection_error += rhs.reprojection_error;
|
||||
num_points += rhs.num_points;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename Scalar>
|
||||
struct ComputeErrorPosesOpt : public LinearizeBase<Scalar> {
|
||||
static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
|
||||
|
||||
typedef Sophus::SE3<Scalar> SE3;
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
|
||||
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<AprilgridCornersData>::const_iterator
|
||||
AprilgridCornersDataIter;
|
||||
|
||||
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
|
||||
|
||||
Scalar error;
|
||||
Scalar reprojection_error;
|
||||
int num_points;
|
||||
|
||||
size_t opt_size;
|
||||
|
||||
const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
|
||||
|
||||
ComputeErrorPosesOpt(
|
||||
size_t opt_size,
|
||||
const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose,
|
||||
const CalibCommonData& common_data)
|
||||
: opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
|
||||
this->common_data = common_data;
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
}
|
||||
|
||||
ComputeErrorPosesOpt(const ComputeErrorPosesOpt& other, tbb::split)
|
||||
: opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) {
|
||||
this->common_data = other.common_data;
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
|
||||
for (const AprilgridCornersData& acd : r) {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
SE3 T_w_i = timestam_to_pose.at(acd.timestamp_ns);
|
||||
SE3 T_w_c =
|
||||
T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
|
||||
SE3 T_c_w = T_w_c.inverse();
|
||||
Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
|
||||
|
||||
double err = 0;
|
||||
double reproj_err = 0;
|
||||
int num_inliers = 0;
|
||||
|
||||
for (size_t i = 0; i < acd.corner_pos.size(); i++) {
|
||||
this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
|
||||
T_c_w_m, cam, nullptr, err, num_inliers,
|
||||
reproj_err);
|
||||
}
|
||||
|
||||
error += err;
|
||||
reprojection_error += reproj_err;
|
||||
num_points += num_inliers;
|
||||
},
|
||||
this->common_data.calibration->intrinsics[acd.cam_id].variant);
|
||||
}
|
||||
}
|
||||
|
||||
void join(ComputeErrorPosesOpt& rhs) {
|
||||
error += rhs.error;
|
||||
reprojection_error += rhs.reprojection_error;
|
||||
num_points += rhs.num_points;
|
||||
}
|
||||
}; // namespace basalt
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
#endif
|
||||
315
include/basalt/optimization/poses_optimize.h
Normal file
315
include/basalt/optimization/poses_optimize.h
Normal file
@@ -0,0 +1,315 @@
|
||||
/**
|
||||
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/calibration/aprilgrid.h>
|
||||
#include <basalt/calibration/calibration_helper.h>
|
||||
#include <basalt/optimization/poses_linearize.h>
|
||||
|
||||
#include <tbb/parallel_reduce.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
class PosesOptimization {
|
||||
static constexpr size_t POSE_SIZE = 6;
|
||||
|
||||
using Scalar = double;
|
||||
|
||||
typedef LinearizePosesOpt<Scalar, SparseHashAccumulator<Scalar>> LinearizeT;
|
||||
|
||||
using SE3 = typename LinearizeT::SE3;
|
||||
using Vector2 = typename LinearizeT::Vector2;
|
||||
using Vector3 = typename LinearizeT::Vector3;
|
||||
using Vector4 = typename LinearizeT::Vector4;
|
||||
using VectorX = typename LinearizeT::VectorX;
|
||||
|
||||
using AprilgridCornersDataIter =
|
||||
typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator;
|
||||
|
||||
public:
|
||||
PosesOptimization()
|
||||
: lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {}
|
||||
|
||||
Vector2 getOpticalCenter(size_t i) {
|
||||
return calib->intrinsics[i].getParam().template segment<2>(2);
|
||||
}
|
||||
|
||||
void resetCalib(size_t num_cams, const std::vector<std::string> &cam_types) {
|
||||
BASALT_ASSERT(cam_types.size() == num_cams);
|
||||
|
||||
calib.reset(new Calibration<Scalar>);
|
||||
|
||||
for (size_t i = 0; i < cam_types.size(); i++) {
|
||||
calib->intrinsics.emplace_back(
|
||||
GenericCamera<Scalar>::fromString(cam_types[i]));
|
||||
|
||||
if (calib->intrinsics.back().getName() != cam_types[i]) {
|
||||
std::cerr << "Unknown camera type " << cam_types[i] << " default to "
|
||||
<< calib->intrinsics.back().getName() << std::endl;
|
||||
}
|
||||
}
|
||||
calib->T_i_c.resize(num_cams);
|
||||
}
|
||||
|
||||
void loadCalib(const std::string &p) {
|
||||
std::string path = p + "calibration.json";
|
||||
|
||||
std::ifstream is(path);
|
||||
|
||||
if (is.good()) {
|
||||
cereal::JSONInputArchive archive(is);
|
||||
calib.reset(new Calibration<Scalar>);
|
||||
archive(*calib);
|
||||
std::cout << "Loaded calibration from: " << path << std::endl;
|
||||
} else {
|
||||
std::cout << "No calibration found" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void saveCalib(const std::string &path) const {
|
||||
if (calib) {
|
||||
std::ofstream os(path + "calibration.json");
|
||||
cereal::JSONOutputArchive archive(os);
|
||||
|
||||
archive(*calib);
|
||||
}
|
||||
}
|
||||
|
||||
bool calibInitialized() const { return calib != nullptr; }
|
||||
bool initialized() const { return true; }
|
||||
|
||||
// Returns true when converged
|
||||
bool optimize(bool opt_intrinsics, double huber_thresh, double stop_thresh,
|
||||
double &error, int &num_points, double &reprojection_error) {
|
||||
error = 0;
|
||||
num_points = 0;
|
||||
|
||||
ccd.opt_intrinsics = opt_intrinsics;
|
||||
ccd.huber_thresh = huber_thresh;
|
||||
|
||||
LinearizePosesOpt<double, SparseHashAccumulator<double>> lopt(
|
||||
problem_size, timestam_to_pose, ccd);
|
||||
|
||||
tbb::blocked_range<AprilgridCornersDataIter> april_range(
|
||||
aprilgrid_corners_measurements.begin(),
|
||||
aprilgrid_corners_measurements.end());
|
||||
tbb::parallel_reduce(april_range, lopt);
|
||||
|
||||
error = lopt.error;
|
||||
num_points = lopt.num_points;
|
||||
reprojection_error = lopt.reprojection_error;
|
||||
|
||||
std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
|
||||
<< lopt.num_points << std::endl;
|
||||
|
||||
lopt.accum.setup_solver();
|
||||
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
|
||||
|
||||
bool converged = false;
|
||||
bool step = false;
|
||||
int max_iter = 10;
|
||||
|
||||
while (!step && max_iter > 0 && !converged) {
|
||||
Eigen::aligned_unordered_map<int64_t, Sophus::SE3d>
|
||||
timestam_to_pose_backup = timestam_to_pose;
|
||||
Calibration<Scalar> calib_backup = *calib;
|
||||
|
||||
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
|
||||
for (int i = 0; i < Hdiag_lambda.size(); i++)
|
||||
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
|
||||
|
||||
Eigen::VectorXd inc = -lopt.accum.solve(&Hdiag_lambda);
|
||||
double max_inc = inc.array().abs().maxCoeff();
|
||||
if (max_inc < stop_thresh) converged = true;
|
||||
|
||||
for (auto &kv : timestam_to_pose) {
|
||||
kv.second *=
|
||||
Sophus::se3_expd(inc.segment<POSE_SIZE>(offset_poses[kv.first]));
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < calib->T_i_c.size(); i++) {
|
||||
calib->T_i_c[i] *=
|
||||
Sophus::se3_expd(inc.segment<POSE_SIZE>(offset_T_i_c[i]));
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < calib->intrinsics.size(); i++) {
|
||||
auto &c = calib->intrinsics[i];
|
||||
c.applyInc(inc.segment(offset_cam_intrinsics[i], c.getN()));
|
||||
}
|
||||
|
||||
ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
|
||||
tbb::parallel_reduce(april_range, eopt);
|
||||
|
||||
double f_diff = (lopt.error - eopt.error);
|
||||
double l_diff = 0.5 * inc.dot(inc * lambda - lopt.accum.getB());
|
||||
|
||||
// std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
|
||||
|
||||
double step_quality = f_diff / l_diff;
|
||||
|
||||
if (step_quality < 0) {
|
||||
std::cout << "\t[REJECTED] lambda:" << lambda
|
||||
<< " step_quality: " << step_quality
|
||||
<< " max_inc: " << max_inc << " Error: " << eopt.error
|
||||
<< " num points " << eopt.num_points << std::endl;
|
||||
lambda = std::min(max_lambda, lambda_vee * lambda);
|
||||
lambda_vee *= 2;
|
||||
|
||||
timestam_to_pose = timestam_to_pose_backup;
|
||||
*calib = calib_backup;
|
||||
} else {
|
||||
std::cout << "\t[ACCEPTED] lambda:" << lambda
|
||||
<< " step_quality: " << step_quality
|
||||
<< " max_inc: " << max_inc << " Error: " << eopt.error
|
||||
<< " num points " << eopt.num_points << std::endl;
|
||||
|
||||
lambda = std::max(
|
||||
min_lambda,
|
||||
lambda *
|
||||
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
|
||||
lambda_vee = 2;
|
||||
|
||||
error = eopt.error;
|
||||
num_points = eopt.num_points;
|
||||
reprojection_error = eopt.reprojection_error;
|
||||
|
||||
step = true;
|
||||
}
|
||||
max_iter--;
|
||||
}
|
||||
|
||||
if (converged) {
|
||||
std::cout << "[CONVERGED]" << std::endl;
|
||||
}
|
||||
|
||||
return converged;
|
||||
}
|
||||
|
||||
void recompute_size() {
|
||||
offset_poses.clear();
|
||||
offset_T_i_c.clear();
|
||||
offset_cam_intrinsics.clear();
|
||||
|
||||
size_t curr_offset = 0;
|
||||
|
||||
for (const auto &kv : timestam_to_pose) {
|
||||
offset_poses[kv.first] = curr_offset;
|
||||
curr_offset += POSE_SIZE;
|
||||
}
|
||||
|
||||
offset_T_i_c.emplace_back(curr_offset);
|
||||
for (size_t i = 0; i < calib->T_i_c.size(); i++)
|
||||
offset_T_i_c.emplace_back(offset_T_i_c.back() + POSE_SIZE);
|
||||
|
||||
offset_cam_intrinsics.emplace_back(offset_T_i_c.back());
|
||||
for (size_t i = 0; i < calib->intrinsics.size(); i++)
|
||||
offset_cam_intrinsics.emplace_back(offset_cam_intrinsics.back() +
|
||||
calib->intrinsics[i].getN());
|
||||
|
||||
problem_size = offset_cam_intrinsics.back();
|
||||
}
|
||||
|
||||
Sophus::SE3d getT_w_i(int64_t i) {
|
||||
auto it = timestam_to_pose.find(i);
|
||||
|
||||
if (it == timestam_to_pose.end())
|
||||
return Sophus::SE3d();
|
||||
else
|
||||
return it->second;
|
||||
}
|
||||
|
||||
void setAprilgridCorners3d(const Eigen::aligned_vector<Eigen::Vector4d> &v) {
|
||||
aprilgrid_corner_pos_3d = v;
|
||||
}
|
||||
|
||||
void init() {
|
||||
recompute_size();
|
||||
|
||||
ccd.calibration = calib.get();
|
||||
ccd.aprilgrid_corner_pos_3d = &aprilgrid_corner_pos_3d;
|
||||
ccd.offset_poses = &offset_poses;
|
||||
ccd.offset_T_i_c = &offset_T_i_c;
|
||||
ccd.offset_intrinsics = &offset_cam_intrinsics;
|
||||
}
|
||||
|
||||
void addAprilgridMeasurement(
|
||||
int64_t t_ns, int cam_id,
|
||||
const Eigen::aligned_vector<Eigen::Vector2d> &corners_pos,
|
||||
const std::vector<int> &corner_id) {
|
||||
aprilgrid_corners_measurements.emplace_back();
|
||||
|
||||
aprilgrid_corners_measurements.back().timestamp_ns = t_ns;
|
||||
aprilgrid_corners_measurements.back().cam_id = cam_id;
|
||||
aprilgrid_corners_measurements.back().corner_pos = corners_pos;
|
||||
aprilgrid_corners_measurements.back().corner_id = corner_id;
|
||||
}
|
||||
|
||||
void addPoseMeasurement(int64_t t_ns, const Sophus::SE3d &pose) {
|
||||
timestam_to_pose[t_ns] = pose;
|
||||
}
|
||||
|
||||
void setVignette(const std::vector<basalt::RdSpline<1, 4>> &vign) {
|
||||
calib->vignette = vign;
|
||||
}
|
||||
|
||||
void setResolution(const Eigen::aligned_vector<Eigen::Vector2i> &resolution) {
|
||||
calib->resolution = resolution;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
std::shared_ptr<Calibration<Scalar>> calib;
|
||||
|
||||
private:
|
||||
typename LinearizePosesOpt<
|
||||
Scalar, SparseHashAccumulator<Scalar>>::CalibCommonData ccd;
|
||||
|
||||
Scalar lambda, min_lambda, max_lambda, lambda_vee;
|
||||
|
||||
size_t problem_size;
|
||||
|
||||
std::unordered_map<int64_t, size_t> offset_poses;
|
||||
std::vector<size_t> offset_cam_intrinsics;
|
||||
std::vector<size_t> offset_T_i_c;
|
||||
|
||||
// frame poses
|
||||
Eigen::aligned_unordered_map<int64_t, Sophus::SE3d> timestam_to_pose;
|
||||
|
||||
Eigen::aligned_vector<AprilgridCornersData> aprilgrid_corners_measurements;
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
846
include/basalt/optimization/spline_linearize.h
Normal file
846
include/basalt/optimization/spline_linearize.h
Normal file
@@ -0,0 +1,846 @@
|
||||
/**
|
||||
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_SPLINE_LINEARIZE_H
|
||||
#define BASALT_SPLINE_LINEARIZE_H
|
||||
|
||||
#include <basalt/io/dataset_io.h>
|
||||
#include <basalt/spline/se3_spline.h>
|
||||
#include <basalt/camera/generic_camera.hpp>
|
||||
#include <basalt/camera/stereographic_param.hpp>
|
||||
|
||||
#include <basalt/optimization/linearize.h>
|
||||
|
||||
#include <basalt/utils/test_utils.h>
|
||||
|
||||
#include <tbb/blocked_range.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <int N, typename Scalar, typename AccumT>
|
||||
struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
|
||||
static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
|
||||
static const int POS_SIZE = LinearizeBase<Scalar>::POS_SIZE;
|
||||
static const int POS_OFFSET = LinearizeBase<Scalar>::POS_OFFSET;
|
||||
static const int ROT_SIZE = LinearizeBase<Scalar>::ROT_SIZE;
|
||||
static const int ROT_OFFSET = LinearizeBase<Scalar>::ROT_OFFSET;
|
||||
static const int ACCEL_BIAS_SIZE = LinearizeBase<Scalar>::ACCEL_BIAS_SIZE;
|
||||
static const int GYRO_BIAS_SIZE = LinearizeBase<Scalar>::GYRO_BIAS_SIZE;
|
||||
static const int G_SIZE = LinearizeBase<Scalar>::G_SIZE;
|
||||
static const int TIME_SIZE = LinearizeBase<Scalar>::TIME_SIZE;
|
||||
static const int ACCEL_BIAS_OFFSET = LinearizeBase<Scalar>::ACCEL_BIAS_OFFSET;
|
||||
static const int GYRO_BIAS_OFFSET = LinearizeBase<Scalar>::GYRO_BIAS_OFFSET;
|
||||
static const int G_OFFSET = LinearizeBase<Scalar>::G_OFFSET;
|
||||
|
||||
typedef Sophus::SE3<Scalar> SE3;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
|
||||
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, 2, 4> Matrix24;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
|
||||
|
||||
typedef Se3Spline<N, Scalar> SplineT;
|
||||
|
||||
typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
|
||||
typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
|
||||
typedef
|
||||
typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
|
||||
typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
|
||||
AprilgridCornersDataIter;
|
||||
typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
|
||||
MocapPoseDataIter;
|
||||
|
||||
// typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
|
||||
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
|
||||
|
||||
AccumT accum;
|
||||
Scalar error;
|
||||
Scalar reprojection_error;
|
||||
int num_points;
|
||||
|
||||
size_t opt_size;
|
||||
|
||||
const SplineT* spline;
|
||||
|
||||
LinearizeSplineOpt(size_t opt_size, const SplineT* spl,
|
||||
const CalibCommonData& common_data)
|
||||
: opt_size(opt_size), spline(spl) {
|
||||
this->common_data = common_data;
|
||||
|
||||
accum.reset(opt_size);
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
|
||||
BASALT_ASSERT(spline);
|
||||
}
|
||||
|
||||
LinearizeSplineOpt(const LinearizeSplineOpt& other, tbb::split)
|
||||
: opt_size(other.opt_size), spline(other.spline) {
|
||||
this->common_data = other.common_data;
|
||||
accum.reset(opt_size);
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<PoseDataIter>& r) {
|
||||
for (const PoseData& pm : r) {
|
||||
int64_t start_idx;
|
||||
|
||||
typename SplineT::SO3JacobianStruct J_rot;
|
||||
typename SplineT::PosJacobianStruct J_pos;
|
||||
|
||||
int64_t time_ns = pm.timestamp_ns;
|
||||
|
||||
// std::cout << "time " << time << std::endl;
|
||||
// std::cout << "sline.minTime() " << spline.minTime() << std::endl;
|
||||
|
||||
BASALT_ASSERT_STREAM(
|
||||
time_ns >= spline->minTimeNs(),
|
||||
"time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
|
||||
|
||||
// Residual from current value of spline
|
||||
Vector3 residual_pos =
|
||||
spline->positionResidual(time_ns, pm.data.translation(), &J_pos);
|
||||
Vector3 residual_rot =
|
||||
spline->orientationResidual(time_ns, pm.data.so3(), &J_rot);
|
||||
|
||||
// std::cerr << "residual_pos " << residual_pos.transpose() << std::endl;
|
||||
// std::cerr << "residual_rot " << residual_rot.transpose() << std::endl;
|
||||
|
||||
BASALT_ASSERT(J_pos.start_idx == J_rot.start_idx);
|
||||
|
||||
start_idx = J_pos.start_idx;
|
||||
|
||||
// std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl;
|
||||
|
||||
const Scalar& pose_var_inv = this->common_data.pose_var_inv;
|
||||
|
||||
error += pose_var_inv *
|
||||
(residual_pos.squaredNorm() + residual_rot.squaredNorm());
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
size_t start_i = (start_idx + i) * POSE_SIZE;
|
||||
|
||||
// std::cout << "start_idx " << start_idx << std::endl;
|
||||
// std::cout << "start_i " << start_i << std::endl;
|
||||
|
||||
BASALT_ASSERT(start_i < opt_size);
|
||||
|
||||
for (size_t j = 0; j <= i; j++) {
|
||||
size_t start_j = (start_idx + j) * POSE_SIZE;
|
||||
|
||||
BASALT_ASSERT(start_j < opt_size);
|
||||
|
||||
accum.template addH<POS_SIZE, POS_SIZE>(
|
||||
start_i + POS_OFFSET, start_j + POS_OFFSET,
|
||||
this->common_data.pose_var_inv * J_pos.d_val_d_knot[i] *
|
||||
J_pos.d_val_d_knot[j] * Matrix3::Identity());
|
||||
|
||||
accum.template addH<ROT_SIZE, ROT_SIZE>(
|
||||
start_i + ROT_OFFSET, start_j + ROT_OFFSET,
|
||||
this->common_data.pose_var_inv *
|
||||
J_rot.d_val_d_knot[i].transpose() * J_rot.d_val_d_knot[j]);
|
||||
}
|
||||
|
||||
accum.template addB<POS_SIZE>(
|
||||
start_i + POS_OFFSET,
|
||||
pose_var_inv * J_pos.d_val_d_knot[i] * residual_pos);
|
||||
accum.template addB<ROT_SIZE>(
|
||||
start_i + ROT_OFFSET,
|
||||
pose_var_inv * J_rot.d_val_d_knot[i].transpose() * residual_rot);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<AccelDataIter>& r) {
|
||||
// size_t num_knots = spline.numKnots();
|
||||
// size_t bias_block_offset = POSE_SIZE * num_knots;
|
||||
|
||||
for (const AccelData& pm : r) {
|
||||
typename SplineT::AccelPosSO3JacobianStruct J;
|
||||
typename SplineT::Mat39 J_bias;
|
||||
Matrix3 J_g;
|
||||
|
||||
int64_t t = pm.timestamp_ns;
|
||||
|
||||
// std::cout << "time " << t << std::endl;
|
||||
// std::cout << "sline.minTime() " << spline.minTime() << std::endl;
|
||||
|
||||
BASALT_ASSERT_STREAM(
|
||||
t >= spline->minTimeNs(),
|
||||
"t " << t << " spline.minTime() " << spline->minTimeNs());
|
||||
BASALT_ASSERT_STREAM(
|
||||
t <= spline->maxTimeNs(),
|
||||
"t " << t << " spline.maxTime() " << spline->maxTimeNs());
|
||||
|
||||
Vector3 residual = spline->accelResidual(
|
||||
t, pm.data, this->common_data.calibration->calib_accel_bias,
|
||||
*(this->common_data.g), &J, &J_bias, &J_g);
|
||||
|
||||
if (!this->common_data.opt_imu_scale) {
|
||||
J_bias.template block<3, 6>(0, 3).setZero();
|
||||
}
|
||||
|
||||
// std::cerr << "================================" << std::endl;
|
||||
// std::cerr << "accel res: " << residual.transpose() << std::endl;
|
||||
// std::cerr << "accel_bias.transpose(): "
|
||||
// << this->common_data.calibration->accel_bias.transpose()
|
||||
// << std::endl;
|
||||
// std::cerr << "*(this->common_data.g): "
|
||||
// << this->common_data.g->transpose() << std::endl;
|
||||
// std::cerr << "pm.data " << pm.data.transpose() << std::endl;
|
||||
|
||||
// std::cerr << "time " << t << std::endl;
|
||||
// std::cerr << "sline.minTime() " << spline.minTime() << std::endl;
|
||||
// std::cerr << "sline.maxTime() " << spline.maxTime() << std::endl;
|
||||
// std::cerr << "================================" << std::endl;
|
||||
|
||||
const Vector3& accel_var_inv = this->common_data.accel_var_inv;
|
||||
|
||||
error += residual.transpose() * accel_var_inv.asDiagonal() * residual;
|
||||
|
||||
size_t start_bias =
|
||||
this->common_data.bias_block_offset + ACCEL_BIAS_OFFSET;
|
||||
size_t start_g = this->common_data.bias_block_offset + G_OFFSET;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
size_t start_i = (J.start_idx + i) * POSE_SIZE;
|
||||
|
||||
BASALT_ASSERT(start_i < opt_size);
|
||||
|
||||
for (size_t j = 0; j <= i; j++) {
|
||||
size_t start_j = (J.start_idx + j) * POSE_SIZE;
|
||||
|
||||
BASALT_ASSERT(start_j < opt_size);
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_i, start_j,
|
||||
J.d_val_d_knot[i].transpose() * accel_var_inv.asDiagonal() *
|
||||
J.d_val_d_knot[j]);
|
||||
}
|
||||
accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
|
||||
start_bias, start_i,
|
||||
J_bias.transpose() * accel_var_inv.asDiagonal() *
|
||||
J.d_val_d_knot[i]);
|
||||
|
||||
if (this->common_data.opt_g) {
|
||||
accum.template addH<G_SIZE, POSE_SIZE>(
|
||||
start_g, start_i,
|
||||
J_g.transpose() * accel_var_inv.asDiagonal() * J.d_val_d_knot[i]);
|
||||
}
|
||||
|
||||
accum.template addB<POSE_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
|
||||
accel_var_inv.asDiagonal() *
|
||||
residual);
|
||||
}
|
||||
|
||||
accum.template addH<ACCEL_BIAS_SIZE, ACCEL_BIAS_SIZE>(
|
||||
start_bias, start_bias,
|
||||
J_bias.transpose() * accel_var_inv.asDiagonal() * J_bias);
|
||||
|
||||
if (this->common_data.opt_g) {
|
||||
accum.template addH<G_SIZE, ACCEL_BIAS_SIZE>(
|
||||
start_g, start_bias,
|
||||
J_g.transpose() * accel_var_inv.asDiagonal() * J_bias);
|
||||
accum.template addH<G_SIZE, G_SIZE>(
|
||||
start_g, start_g,
|
||||
J_g.transpose() * accel_var_inv.asDiagonal() * J_g);
|
||||
}
|
||||
|
||||
accum.template addB<ACCEL_BIAS_SIZE>(
|
||||
start_bias,
|
||||
J_bias.transpose() * accel_var_inv.asDiagonal() * residual);
|
||||
|
||||
if (this->common_data.opt_g) {
|
||||
accum.template addB<G_SIZE>(
|
||||
start_g, J_g.transpose() * accel_var_inv.asDiagonal() * residual);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<GyroDataIter>& r) {
|
||||
// size_t num_knots = spline.numKnots();
|
||||
// size_t bias_block_offset = POSE_SIZE * num_knots;
|
||||
|
||||
for (const GyroData& pm : r) {
|
||||
typename SplineT::SO3JacobianStruct J;
|
||||
typename SplineT::Mat312 J_bias;
|
||||
|
||||
int64_t t_ns = pm.timestamp_ns;
|
||||
|
||||
BASALT_ASSERT(t_ns >= spline->minTimeNs());
|
||||
BASALT_ASSERT(t_ns <= spline->maxTimeNs());
|
||||
|
||||
const Vector3& gyro_var_inv = this->common_data.gyro_var_inv;
|
||||
|
||||
Vector3 residual = spline->gyroResidual(
|
||||
t_ns, pm.data, this->common_data.calibration->calib_gyro_bias, &J,
|
||||
&J_bias);
|
||||
|
||||
if (!this->common_data.opt_imu_scale) {
|
||||
J_bias.template block<3, 9>(0, 3).setZero();
|
||||
}
|
||||
|
||||
// std::cerr << "==============================" << std::endl;
|
||||
// std::cerr << "residual " << residual.transpose() << std::endl;
|
||||
// std::cerr << "gyro_bias "
|
||||
// << this->common_data.calibration->gyro_bias.transpose()
|
||||
// << std::endl;
|
||||
// std::cerr << "pm.data " << pm.data.transpose() << std::endl;
|
||||
// std::cerr << "t_ns " << t_ns << std::endl;
|
||||
|
||||
error += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
|
||||
|
||||
size_t start_bias =
|
||||
this->common_data.bias_block_offset + GYRO_BIAS_OFFSET;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
size_t start_i = (J.start_idx + i) * POSE_SIZE + ROT_OFFSET;
|
||||
|
||||
// std::cout << "start_idx " << J.start_idx << std::endl;
|
||||
// std::cout << "start_i " << start_i << std::endl;
|
||||
|
||||
BASALT_ASSERT(start_i < opt_size);
|
||||
|
||||
for (size_t j = 0; j <= i; j++) {
|
||||
size_t start_j = (J.start_idx + j) * POSE_SIZE + ROT_OFFSET;
|
||||
|
||||
// std::cout << "start_j " << start_j << std::endl;
|
||||
|
||||
BASALT_ASSERT(start_i < opt_size);
|
||||
|
||||
accum.template addH<ROT_SIZE, ROT_SIZE>(
|
||||
start_i, start_j,
|
||||
J.d_val_d_knot[i].transpose() * gyro_var_inv.asDiagonal() *
|
||||
J.d_val_d_knot[j]);
|
||||
}
|
||||
accum.template addH<GYRO_BIAS_SIZE, ROT_SIZE>(
|
||||
start_bias, start_i,
|
||||
J_bias.transpose() * gyro_var_inv.asDiagonal() * J.d_val_d_knot[i]);
|
||||
accum.template addB<ROT_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
|
||||
gyro_var_inv.asDiagonal() *
|
||||
residual);
|
||||
}
|
||||
|
||||
accum.template addH<GYRO_BIAS_SIZE, GYRO_BIAS_SIZE>(
|
||||
start_bias, start_bias,
|
||||
J_bias.transpose() * gyro_var_inv.asDiagonal() * J_bias);
|
||||
accum.template addB<GYRO_BIAS_SIZE>(
|
||||
start_bias,
|
||||
J_bias.transpose() * gyro_var_inv.asDiagonal() * residual);
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
|
||||
for (const AprilgridCornersData& acd : r) {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
constexpr int INTRINSICS_SIZE =
|
||||
std::remove_reference<decltype(cam)>::type::N;
|
||||
|
||||
typename SplineT::PosePosSO3JacobianStruct J;
|
||||
|
||||
int64_t time_ns = acd.timestamp_ns +
|
||||
this->common_data.calibration->cam_time_offset_ns;
|
||||
|
||||
if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
|
||||
return;
|
||||
|
||||
SE3 T_w_i = spline->pose(time_ns, &J);
|
||||
|
||||
Vector6 d_T_wi_d_time;
|
||||
spline->d_pose_d_t(time_ns, d_T_wi_d_time);
|
||||
|
||||
SE3 T_w_c =
|
||||
T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
|
||||
SE3 T_c_w = T_w_c.inverse();
|
||||
Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
|
||||
|
||||
typename LinearizeBase<Scalar>::template PoseCalibH<INTRINSICS_SIZE>
|
||||
cph;
|
||||
|
||||
double err = 0;
|
||||
double reproj_err = 0;
|
||||
int num_inliers = 0;
|
||||
|
||||
for (size_t i = 0; i < acd.corner_pos.size(); i++) {
|
||||
this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
|
||||
T_c_w_m, cam, &cph, err, num_inliers,
|
||||
reproj_err);
|
||||
}
|
||||
|
||||
error += err;
|
||||
reprojection_error += reproj_err;
|
||||
num_points += num_inliers;
|
||||
|
||||
Matrix6 Adj = -this->common_data.calibration->T_i_c[acd.cam_id]
|
||||
.inverse()
|
||||
.Adj();
|
||||
Matrix6 A_H_p_A = Adj.transpose() * cph.H_pose_accum * Adj;
|
||||
|
||||
size_t T_i_c_start = this->common_data.offset_T_i_c->at(acd.cam_id);
|
||||
size_t calib_start =
|
||||
this->common_data.offset_intrinsics->at(acd.cam_id);
|
||||
size_t start_cam_time_offset =
|
||||
this->common_data.mocap_block_offset + 2 * POSE_SIZE + 1;
|
||||
|
||||
for (int i = 0; i < N; i++) {
|
||||
int start_i = (J.start_idx + i) * POSE_SIZE;
|
||||
|
||||
Matrix6 Ji_A_H_p_A = J.d_val_d_knot[i].transpose() * A_H_p_A;
|
||||
|
||||
for (int j = 0; j <= i; j++) {
|
||||
int start_j = (J.start_idx + j) * POSE_SIZE;
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_i, start_j, Ji_A_H_p_A * J.d_val_d_knot[j]);
|
||||
}
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
T_i_c_start, start_i,
|
||||
-cph.H_pose_accum * Adj * J.d_val_d_knot[i]);
|
||||
|
||||
if (this->common_data.opt_intrinsics)
|
||||
accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
|
||||
calib_start, start_i,
|
||||
cph.H_intr_pose_accum * Adj * J.d_val_d_knot[i]);
|
||||
|
||||
if (this->common_data.opt_cam_time_offset)
|
||||
accum.template addH<TIME_SIZE, POSE_SIZE>(
|
||||
start_cam_time_offset, start_i,
|
||||
d_T_wi_d_time.transpose() * A_H_p_A * J.d_val_d_knot[i]);
|
||||
|
||||
accum.template addB<POSE_SIZE>(
|
||||
start_i, J.d_val_d_knot[i].transpose() * Adj.transpose() *
|
||||
cph.b_pose_accum);
|
||||
}
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(T_i_c_start, T_i_c_start,
|
||||
cph.H_pose_accum);
|
||||
accum.template addB<POSE_SIZE>(T_i_c_start, -cph.b_pose_accum);
|
||||
|
||||
if (this->common_data.opt_intrinsics) {
|
||||
accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
|
||||
calib_start, T_i_c_start, -cph.H_intr_pose_accum);
|
||||
accum.template addH<INTRINSICS_SIZE, INTRINSICS_SIZE>(
|
||||
calib_start, calib_start, cph.H_intr_accum);
|
||||
accum.template addB<INTRINSICS_SIZE>(calib_start,
|
||||
cph.b_intr_accum);
|
||||
}
|
||||
|
||||
if (this->common_data.opt_cam_time_offset) {
|
||||
accum.template addH<TIME_SIZE, POSE_SIZE>(
|
||||
start_cam_time_offset, T_i_c_start,
|
||||
-d_T_wi_d_time.transpose() * Adj.transpose() *
|
||||
cph.H_pose_accum);
|
||||
|
||||
if (this->common_data.opt_intrinsics)
|
||||
accum.template addH<TIME_SIZE, INTRINSICS_SIZE>(
|
||||
start_cam_time_offset, calib_start,
|
||||
d_T_wi_d_time.transpose() * Adj.transpose() *
|
||||
cph.H_intr_pose_accum.transpose());
|
||||
|
||||
accum.template addH<TIME_SIZE, TIME_SIZE>(
|
||||
start_cam_time_offset, start_cam_time_offset,
|
||||
d_T_wi_d_time.transpose() * A_H_p_A * d_T_wi_d_time);
|
||||
|
||||
accum.template addB<TIME_SIZE>(start_cam_time_offset,
|
||||
d_T_wi_d_time.transpose() *
|
||||
Adj.transpose() *
|
||||
cph.b_pose_accum);
|
||||
}
|
||||
},
|
||||
this->common_data.calibration->intrinsics[acd.cam_id].variant);
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<MocapPoseDataIter>& r) {
|
||||
for (const MocapPoseData& pm : r) {
|
||||
typename SplineT::PosePosSO3JacobianStruct J_pose;
|
||||
|
||||
int64_t time_ns =
|
||||
pm.timestamp_ns +
|
||||
this->common_data.mocap_calibration->mocap_time_offset_ns;
|
||||
|
||||
// std::cout << "time " << time << std::endl;
|
||||
// std::cout << "sline.minTime() " << spline.minTime() << std::endl;
|
||||
|
||||
if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
|
||||
continue;
|
||||
|
||||
BASALT_ASSERT_STREAM(
|
||||
time_ns >= spline->minTimeNs(),
|
||||
"time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
|
||||
|
||||
const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w;
|
||||
const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark;
|
||||
|
||||
const SE3 T_w_i = spline->pose(time_ns, &J_pose);
|
||||
const SE3 T_moc_mark = T_moc_w * T_w_i * T_i_mark;
|
||||
|
||||
const SE3 T_mark_moc_meas = pm.data.inverse();
|
||||
|
||||
Vector6 residual = Sophus::se3_logd(T_mark_moc_meas * T_moc_mark);
|
||||
|
||||
// TODO: check derivatives
|
||||
Matrix6 d_res_d_T_i_mark;
|
||||
Sophus::rightJacobianInvSE3Decoupled(residual, d_res_d_T_i_mark);
|
||||
Matrix6 d_res_d_T_w_i = d_res_d_T_i_mark * T_i_mark.inverse().Adj();
|
||||
Matrix6 d_res_d_T_moc_w =
|
||||
d_res_d_T_i_mark * (T_w_i * T_i_mark).inverse().Adj();
|
||||
|
||||
Vector6 d_T_wi_d_time;
|
||||
spline->d_pose_d_t(time_ns, d_T_wi_d_time);
|
||||
|
||||
Vector6 d_res_d_time = d_res_d_T_w_i * d_T_wi_d_time;
|
||||
|
||||
size_t start_idx = J_pose.start_idx;
|
||||
|
||||
size_t start_T_moc_w = this->common_data.mocap_block_offset;
|
||||
size_t start_T_i_mark = this->common_data.mocap_block_offset + POSE_SIZE;
|
||||
size_t start_mocap_time_offset =
|
||||
this->common_data.mocap_block_offset + 2 * POSE_SIZE;
|
||||
|
||||
// std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl;
|
||||
|
||||
const Scalar& mocap_var_inv = this->common_data.mocap_var_inv;
|
||||
|
||||
error += mocap_var_inv * residual.squaredNorm();
|
||||
|
||||
Matrix6 H_T_w_i = d_res_d_T_w_i.transpose() * d_res_d_T_w_i;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
size_t start_i = (start_idx + i) * POSE_SIZE;
|
||||
|
||||
// std::cout << "start_idx " << start_idx << std::endl;
|
||||
// std::cout << "start_i " << start_i << std::endl;
|
||||
|
||||
BASALT_ASSERT(start_i < opt_size);
|
||||
|
||||
Matrix6 Ji_H_T_w_i = J_pose.d_val_d_knot[i].transpose() * H_T_w_i;
|
||||
|
||||
for (size_t j = 0; j <= i; j++) {
|
||||
size_t start_j = (start_idx + j) * POSE_SIZE;
|
||||
|
||||
BASALT_ASSERT(start_j < opt_size);
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_i, start_j,
|
||||
mocap_var_inv * Ji_H_T_w_i * J_pose.d_val_d_knot[j]);
|
||||
}
|
||||
|
||||
accum.template addB<POSE_SIZE>(
|
||||
start_i, mocap_var_inv * J_pose.d_val_d_knot[i].transpose() *
|
||||
d_res_d_T_w_i.transpose() * residual);
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_T_moc_w, start_i,
|
||||
mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_w_i *
|
||||
J_pose.d_val_d_knot[i]);
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_T_i_mark, start_i,
|
||||
mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_w_i *
|
||||
J_pose.d_val_d_knot[i]);
|
||||
|
||||
accum.template addH<TIME_SIZE, POSE_SIZE>(
|
||||
start_mocap_time_offset, start_i,
|
||||
mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_w_i *
|
||||
J_pose.d_val_d_knot[i]);
|
||||
}
|
||||
|
||||
// start_T_moc_w
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_T_moc_w, start_T_moc_w,
|
||||
mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_moc_w);
|
||||
|
||||
// start_T_i_mark
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_T_i_mark, start_T_moc_w,
|
||||
mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_moc_w);
|
||||
|
||||
accum.template addH<POSE_SIZE, POSE_SIZE>(
|
||||
start_T_i_mark, start_T_i_mark,
|
||||
mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_i_mark);
|
||||
|
||||
// start_mocap_time_offset
|
||||
accum.template addH<TIME_SIZE, POSE_SIZE>(
|
||||
start_mocap_time_offset, start_T_moc_w,
|
||||
mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_moc_w);
|
||||
|
||||
accum.template addH<TIME_SIZE, POSE_SIZE>(
|
||||
start_mocap_time_offset, start_T_i_mark,
|
||||
mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_i_mark);
|
||||
|
||||
accum.template addH<TIME_SIZE, TIME_SIZE>(
|
||||
start_mocap_time_offset, start_mocap_time_offset,
|
||||
mocap_var_inv * d_res_d_time.transpose() * d_res_d_time);
|
||||
|
||||
// B
|
||||
accum.template addB<POSE_SIZE>(
|
||||
start_T_moc_w,
|
||||
mocap_var_inv * d_res_d_T_moc_w.transpose() * residual);
|
||||
|
||||
accum.template addB<POSE_SIZE>(
|
||||
start_T_i_mark,
|
||||
mocap_var_inv * d_res_d_T_i_mark.transpose() * residual);
|
||||
|
||||
accum.template addB<TIME_SIZE>(
|
||||
start_mocap_time_offset,
|
||||
mocap_var_inv * d_res_d_time.transpose() * residual);
|
||||
}
|
||||
}
|
||||
|
||||
void join(LinearizeSplineOpt& rhs) {
|
||||
accum.join(rhs.accum);
|
||||
error += rhs.error;
|
||||
reprojection_error += rhs.reprojection_error;
|
||||
num_points += rhs.num_points;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
template <int N, typename Scalar>
|
||||
struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
|
||||
typedef Sophus::SE3<Scalar> SE3;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
|
||||
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
|
||||
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
|
||||
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, 2, 4> Matrix24;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
|
||||
|
||||
typedef Se3Spline<N, Scalar> SplineT;
|
||||
|
||||
typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
|
||||
typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
|
||||
typedef
|
||||
typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
|
||||
typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
|
||||
AprilgridCornersDataIter;
|
||||
typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
|
||||
MocapPoseDataIter;
|
||||
|
||||
// typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
|
||||
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
|
||||
|
||||
Scalar error;
|
||||
Scalar reprojection_error;
|
||||
int num_points;
|
||||
|
||||
size_t opt_size;
|
||||
|
||||
const SplineT* spline;
|
||||
|
||||
ComputeErrorSplineOpt(size_t opt_size, const SplineT* spl,
|
||||
const CalibCommonData& common_data)
|
||||
: opt_size(opt_size), spline(spl) {
|
||||
this->common_data = common_data;
|
||||
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
|
||||
BASALT_ASSERT(spline);
|
||||
}
|
||||
|
||||
ComputeErrorSplineOpt(const ComputeErrorSplineOpt& other, tbb::split)
|
||||
: opt_size(other.opt_size), spline(other.spline) {
|
||||
this->common_data = other.common_data;
|
||||
error = 0;
|
||||
reprojection_error = 0;
|
||||
num_points = 0;
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<PoseDataIter>& r) {
|
||||
for (const PoseData& pm : r) {
|
||||
int64_t time_ns = pm.timestamp_ns;
|
||||
|
||||
BASALT_ASSERT_STREAM(
|
||||
time_ns >= spline->minTimeNs(),
|
||||
"time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
|
||||
|
||||
// Residual from current value of spline
|
||||
Vector3 residual_pos =
|
||||
spline->positionResidual(time_ns, pm.data.translation());
|
||||
Vector3 residual_rot =
|
||||
spline->orientationResidual(time_ns, pm.data.so3());
|
||||
|
||||
// std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl;
|
||||
|
||||
const Scalar& pose_var_inv = this->common_data.pose_var_inv;
|
||||
|
||||
error += pose_var_inv *
|
||||
(residual_pos.squaredNorm() + residual_rot.squaredNorm());
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<AccelDataIter>& r) {
|
||||
// size_t num_knots = spline.numKnots();
|
||||
// size_t bias_block_offset = POSE_SIZE * num_knots;
|
||||
|
||||
for (const AccelData& pm : r) {
|
||||
int64_t t = pm.timestamp_ns;
|
||||
|
||||
// std::cout << "time " << t << std::endl;
|
||||
// std::cout << "sline.minTime() " << spline.minTime() << std::endl;
|
||||
|
||||
BASALT_ASSERT_STREAM(
|
||||
t >= spline->minTimeNs(),
|
||||
"t " << t << " spline.minTime() " << spline->minTimeNs());
|
||||
BASALT_ASSERT_STREAM(
|
||||
t <= spline->maxTimeNs(),
|
||||
"t " << t << " spline.maxTime() " << spline->maxTimeNs());
|
||||
|
||||
Vector3 residual = spline->accelResidual(
|
||||
t, pm.data, this->common_data.calibration->calib_accel_bias,
|
||||
*(this->common_data.g));
|
||||
|
||||
const Vector3& accel_var_inv = this->common_data.accel_var_inv;
|
||||
|
||||
error += residual.transpose() * accel_var_inv.asDiagonal() * residual;
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<GyroDataIter>& r) {
|
||||
// size_t num_knots = spline.numKnots();
|
||||
// size_t bias_block_offset = POSE_SIZE * num_knots;
|
||||
|
||||
for (const GyroData& pm : r) {
|
||||
int64_t t_ns = pm.timestamp_ns;
|
||||
|
||||
BASALT_ASSERT(t_ns >= spline->minTimeNs());
|
||||
BASALT_ASSERT(t_ns <= spline->maxTimeNs());
|
||||
|
||||
const Vector3& gyro_var_inv = this->common_data.gyro_var_inv;
|
||||
|
||||
Vector3 residual = spline->gyroResidual(
|
||||
t_ns, pm.data, this->common_data.calibration->calib_gyro_bias);
|
||||
|
||||
error += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
|
||||
for (const AprilgridCornersData& acd : r) {
|
||||
std::visit(
|
||||
[&](const auto& cam) {
|
||||
int64_t time_ns = acd.timestamp_ns +
|
||||
this->common_data.calibration->cam_time_offset_ns;
|
||||
|
||||
if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
|
||||
return;
|
||||
|
||||
SE3 T_w_i = spline->pose(time_ns);
|
||||
SE3 T_w_c =
|
||||
T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
|
||||
SE3 T_c_w = T_w_c.inverse();
|
||||
Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
|
||||
|
||||
double err = 0;
|
||||
double reproj_err = 0;
|
||||
int num_inliers = 0;
|
||||
|
||||
for (size_t i = 0; i < acd.corner_pos.size(); i++) {
|
||||
this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
|
||||
T_c_w_m, cam, nullptr, err, num_inliers,
|
||||
reproj_err);
|
||||
}
|
||||
|
||||
error += err;
|
||||
reprojection_error += reproj_err;
|
||||
num_points += num_inliers;
|
||||
},
|
||||
this->common_data.calibration->intrinsics[acd.cam_id].variant);
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const tbb::blocked_range<MocapPoseDataIter>& r) {
|
||||
for (const MocapPoseData& pm : r) {
|
||||
int64_t time_ns =
|
||||
pm.timestamp_ns +
|
||||
this->common_data.mocap_calibration->mocap_time_offset_ns;
|
||||
|
||||
if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
|
||||
continue;
|
||||
|
||||
BASALT_ASSERT_STREAM(
|
||||
time_ns >= spline->minTimeNs(),
|
||||
"time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
|
||||
|
||||
const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w;
|
||||
const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark;
|
||||
|
||||
const SE3 T_w_i = spline->pose(time_ns);
|
||||
const SE3 T_moc_mark = T_moc_w * T_w_i * T_i_mark;
|
||||
|
||||
const SE3 T_mark_moc_meas = pm.data.inverse();
|
||||
|
||||
Vector6 residual = Sophus::se3_logd(T_mark_moc_meas * T_moc_mark);
|
||||
|
||||
const Scalar& mocap_var_inv = this->common_data.mocap_var_inv;
|
||||
|
||||
error += mocap_var_inv * residual.squaredNorm();
|
||||
}
|
||||
}
|
||||
|
||||
void join(ComputeErrorSplineOpt& rhs) {
|
||||
error += rhs.error;
|
||||
reprojection_error += rhs.reprojection_error;
|
||||
num_points += rhs.num_points;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
#endif
|
||||
612
include/basalt/optimization/spline_optimize.h
Normal file
612
include/basalt/optimization/spline_optimize.h
Normal file
@@ -0,0 +1,612 @@
|
||||
/**
|
||||
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/optimization/accumulator.h>
|
||||
#include <basalt/optimization/spline_linearize.h>
|
||||
|
||||
#include <basalt/calibration/calibration.hpp>
|
||||
|
||||
#include <basalt/camera/unified_camera.hpp>
|
||||
|
||||
#include <basalt/image/image.h>
|
||||
|
||||
#include <basalt/serialization/headers_serialization.h>
|
||||
|
||||
#include <tbb/parallel_reduce.h>
|
||||
|
||||
#include <chrono>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <int N, typename Scalar>
|
||||
class SplineOptimization {
|
||||
public:
|
||||
typedef LinearizeSplineOpt<N, Scalar, SparseHashAccumulator<Scalar>>
|
||||
LinearizeT;
|
||||
|
||||
typedef typename LinearizeT::SE3 SE3;
|
||||
typedef typename LinearizeT::Vector2 Vector2;
|
||||
typedef typename LinearizeT::Vector3 Vector3;
|
||||
typedef typename LinearizeT::Vector4 Vector4;
|
||||
typedef typename LinearizeT::VectorX VectorX;
|
||||
typedef typename LinearizeT::Matrix24 Matrix24;
|
||||
|
||||
static const int POSE_SIZE = LinearizeT::POSE_SIZE;
|
||||
static const int POS_SIZE = LinearizeT::POS_SIZE;
|
||||
static const int POS_OFFSET = LinearizeT::POS_OFFSET;
|
||||
static const int ROT_SIZE = LinearizeT::ROT_SIZE;
|
||||
static const int ROT_OFFSET = LinearizeT::ROT_OFFSET;
|
||||
static const int ACCEL_BIAS_SIZE = LinearizeT::ACCEL_BIAS_SIZE;
|
||||
static const int GYRO_BIAS_SIZE = LinearizeT::GYRO_BIAS_SIZE;
|
||||
static const int G_SIZE = LinearizeT::G_SIZE;
|
||||
|
||||
static const int ACCEL_BIAS_OFFSET = LinearizeT::ACCEL_BIAS_OFFSET;
|
||||
static const int GYRO_BIAS_OFFSET = LinearizeT::GYRO_BIAS_OFFSET;
|
||||
static const int G_OFFSET = LinearizeT::G_OFFSET;
|
||||
|
||||
const Scalar pose_var;
|
||||
|
||||
Scalar pose_var_inv;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
|
||||
|
||||
typedef Se3Spline<N, Scalar> SplineT;
|
||||
|
||||
SplineOptimization(int64_t dt_ns = 1e7, double init_lambda = 1e-12)
|
||||
: pose_var(1e-4),
|
||||
mocap_initialized(false),
|
||||
lambda(init_lambda),
|
||||
min_lambda(1e-18),
|
||||
max_lambda(100),
|
||||
lambda_vee(2),
|
||||
spline(dt_ns),
|
||||
dt_ns(dt_ns) {
|
||||
pose_var_inv = 1.0 / pose_var;
|
||||
|
||||
// reasonable default values
|
||||
// calib.accelerometer_noise_var = 2e-2;
|
||||
// calib.gyroscope_noise_var = 1e-4;
|
||||
|
||||
g.setZero();
|
||||
|
||||
min_time_us = std::numeric_limits<int64_t>::max();
|
||||
max_time_us = std::numeric_limits<int64_t>::min();
|
||||
}
|
||||
|
||||
int64_t getCamTimeOffsetNs() const { return calib->cam_time_offset_ns; }
|
||||
int64_t getMocapTimeOffsetNs() const {
|
||||
return mocap_calib->mocap_time_offset_ns;
|
||||
}
|
||||
|
||||
const SE3& getCamT_i_c(size_t i) const { return calib->T_i_c[i]; }
|
||||
SE3& getCamT_i_c(size_t i) { return calib->T_i_c[i]; }
|
||||
|
||||
VectorX getIntrinsics(size_t i) const {
|
||||
return calib->intrinsics[i].getParam();
|
||||
}
|
||||
|
||||
const CalibAccelBias<Scalar>& getAccelBias() const {
|
||||
return calib->calib_accel_bias;
|
||||
}
|
||||
const CalibGyroBias<Scalar>& getGyroBias() const {
|
||||
return calib->calib_gyro_bias;
|
||||
}
|
||||
|
||||
void resetCalib(size_t num_cams, const std::vector<std::string>& cam_types) {
|
||||
BASALT_ASSERT(cam_types.size() == num_cams);
|
||||
|
||||
calib.reset(new Calibration<Scalar>);
|
||||
|
||||
calib->intrinsics.resize(num_cams);
|
||||
calib->T_i_c.resize(num_cams);
|
||||
|
||||
mocap_calib.reset(new MocapCalibration<Scalar>);
|
||||
}
|
||||
|
||||
void resetMocapCalib() { mocap_calib.reset(new MocapCalibration<Scalar>); }
|
||||
|
||||
void loadCalib(const std::string& p) {
|
||||
std::string path = p + "calibration.json";
|
||||
|
||||
std::ifstream is(path);
|
||||
|
||||
if (is.good()) {
|
||||
cereal::JSONInputArchive archive(is);
|
||||
calib.reset(new Calibration<Scalar>);
|
||||
archive(*calib);
|
||||
std::cout << "Loaded calibration from: " << path << std::endl;
|
||||
} else {
|
||||
std::cerr << "No calibration found. Run camera calibration first!!!"
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void saveCalib(const std::string& path) const {
|
||||
if (calib) {
|
||||
std::ofstream os(path + "calibration.json");
|
||||
cereal::JSONOutputArchive archive(os);
|
||||
|
||||
archive(*calib);
|
||||
}
|
||||
}
|
||||
|
||||
void saveMocapCalib(const std::string& path,
|
||||
int64_t mocap_to_imu_offset_ns = 0) const {
|
||||
if (calib) {
|
||||
std::ofstream os(path + "mocap_calibration.json");
|
||||
cereal::JSONOutputArchive archive(os);
|
||||
|
||||
mocap_calib->mocap_to_imu_offset_ns = mocap_to_imu_offset_ns;
|
||||
|
||||
archive(*mocap_calib);
|
||||
}
|
||||
}
|
||||
|
||||
bool calibInitialized() const { return calib != nullptr; }
|
||||
|
||||
bool initialized() const { return spline.numKnots() > 0; }
|
||||
|
||||
void initSpline(const SE3& pose, int num_knots) {
|
||||
spline.setKnots(pose, num_knots);
|
||||
}
|
||||
|
||||
void initSpline(const SplineT& other) {
|
||||
spline.setKnots(other);
|
||||
|
||||
// std::cerr << "spline.numKnots() " << spline.numKnots() << std::endl;
|
||||
// std::cerr << "other.numKnots() " << other.numKnots() << std::endl;
|
||||
|
||||
size_t num_knots = spline.numKnots();
|
||||
|
||||
for (size_t i = 0; i < num_knots; i++) {
|
||||
Eigen::Vector3d rot_rand_inc = Eigen::Vector3d::Random() / 20;
|
||||
Eigen::Vector3d trans_rand_inc = Eigen::Vector3d::Random();
|
||||
|
||||
// std::cerr << "rot_rand_inc " << rot_rand_inc.transpose() << std::endl;
|
||||
// std::cerr << "trans_rand_inc " << trans_rand_inc.transpose() <<
|
||||
// std::endl;
|
||||
|
||||
spline.getKnotSO3(i) *= Sophus::SO3d::exp(rot_rand_inc);
|
||||
spline.getKnotPos(i) += trans_rand_inc;
|
||||
}
|
||||
}
|
||||
|
||||
const SplineT& getSpline() const { return spline; }
|
||||
|
||||
Vector3 getG() const { return g; }
|
||||
|
||||
void setG(const Vector3& g_a) { g = g_a; }
|
||||
|
||||
// const Calibration& getCalib() const { return calib; }
|
||||
// void setCalib(const Calibration& c) { calib = c; }
|
||||
|
||||
SE3 getT_w_moc() const { return mocap_calib->T_moc_w.inverse(); }
|
||||
void setT_w_moc(const SE3& val) { mocap_calib->T_moc_w = val.inverse(); }
|
||||
|
||||
SE3 getT_mark_i() const { return mocap_calib->T_i_mark.inverse(); }
|
||||
void setT_mark_i(const SE3& val) { mocap_calib->T_i_mark = val.inverse(); }
|
||||
|
||||
Eigen::Vector3d getTransAccelWorld(int64_t t_ns) const {
|
||||
return spline.transAccelWorld(t_ns);
|
||||
}
|
||||
|
||||
Eigen::Vector3d getRotVelBody(int64_t t_ns) const {
|
||||
return spline.rotVelBody(t_ns);
|
||||
}
|
||||
|
||||
SE3 getT_w_i(int64_t t_ns) { return spline.pose(t_ns); }
|
||||
|
||||
void setAprilgridCorners3d(const Eigen::aligned_vector<Eigen::Vector4d>& v) {
|
||||
aprilgrid_corner_pos_3d = v;
|
||||
}
|
||||
|
||||
void addPoseMeasurement(int64_t t_ns, const SE3& pose) {
|
||||
min_time_us = std::min(min_time_us, t_ns);
|
||||
max_time_us = std::max(max_time_us, t_ns);
|
||||
|
||||
pose_measurements.emplace_back();
|
||||
pose_measurements.back().timestamp_ns = t_ns;
|
||||
pose_measurements.back().data = pose;
|
||||
}
|
||||
|
||||
void addMocapMeasurement(int64_t t_ns, const SE3& pose) {
|
||||
mocap_measurements.emplace_back();
|
||||
mocap_measurements.back().timestamp_ns = t_ns;
|
||||
mocap_measurements.back().data = pose;
|
||||
}
|
||||
|
||||
void addAccelMeasurement(int64_t t_ns, const Vector3& meas) {
|
||||
min_time_us = std::min(min_time_us, t_ns);
|
||||
max_time_us = std::max(max_time_us, t_ns);
|
||||
|
||||
accel_measurements.emplace_back();
|
||||
accel_measurements.back().timestamp_ns = t_ns;
|
||||
accel_measurements.back().data = meas;
|
||||
}
|
||||
|
||||
void addGyroMeasurement(int64_t t_ns, const Vector3& meas) {
|
||||
min_time_us = std::min(min_time_us, t_ns);
|
||||
max_time_us = std::max(max_time_us, t_ns);
|
||||
|
||||
gyro_measurements.emplace_back();
|
||||
gyro_measurements.back().timestamp_ns = t_ns;
|
||||
gyro_measurements.back().data = meas;
|
||||
}
|
||||
|
||||
void addAprilgridMeasurement(
|
||||
int64_t t_ns, int cam_id,
|
||||
const Eigen::aligned_vector<Eigen::Vector2d>& corners_pos,
|
||||
const std::vector<int>& corner_id) {
|
||||
min_time_us = std::min(min_time_us, t_ns);
|
||||
max_time_us = std::max(max_time_us, t_ns);
|
||||
|
||||
aprilgrid_corners_measurements.emplace_back();
|
||||
aprilgrid_corners_measurements.back().timestamp_ns = t_ns;
|
||||
aprilgrid_corners_measurements.back().cam_id = cam_id;
|
||||
aprilgrid_corners_measurements.back().corner_pos = corners_pos;
|
||||
aprilgrid_corners_measurements.back().corner_id = corner_id;
|
||||
}
|
||||
|
||||
Scalar getMinTime() const { return min_time_us * 1e-9; }
|
||||
Scalar getMaxTime() const { return max_time_us * 1e-9; }
|
||||
|
||||
int64_t getMinTimeNs() const { return min_time_us; }
|
||||
int64_t getMaxTimeNs() const { return max_time_us; }
|
||||
|
||||
void init() {
|
||||
int64_t time_interval_us = max_time_us - min_time_us;
|
||||
|
||||
if (spline.numKnots() == 0) {
|
||||
spline.setStartTimeNs(min_time_us);
|
||||
spline.setKnots(pose_measurements.front().data,
|
||||
time_interval_us / dt_ns + N + 1);
|
||||
}
|
||||
|
||||
recompute_size();
|
||||
|
||||
// std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl;
|
||||
// std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl;
|
||||
|
||||
while (!mocap_measurements.empty() &&
|
||||
mocap_measurements.front().timestamp_ns <=
|
||||
spline.minTimeNs() + spline.getDtNs())
|
||||
mocap_measurements.pop_front();
|
||||
|
||||
while (!mocap_measurements.empty() &&
|
||||
mocap_measurements.back().timestamp_ns >=
|
||||
spline.maxTimeNs() - spline.getDtNs())
|
||||
mocap_measurements.pop_back();
|
||||
|
||||
ccd.calibration = calib.get();
|
||||
ccd.mocap_calibration = mocap_calib.get();
|
||||
ccd.aprilgrid_corner_pos_3d = &aprilgrid_corner_pos_3d;
|
||||
ccd.g = &g;
|
||||
ccd.offset_intrinsics = &offset_cam_intrinsics;
|
||||
ccd.offset_T_i_c = &offset_T_i_c;
|
||||
ccd.bias_block_offset = bias_block_offset;
|
||||
ccd.mocap_block_offset = mocap_block_offset;
|
||||
|
||||
ccd.opt_g = true;
|
||||
|
||||
ccd.pose_var_inv = pose_var_inv;
|
||||
ccd.gyro_var_inv =
|
||||
calib->dicrete_time_gyro_noise_std().array().square().inverse();
|
||||
ccd.accel_var_inv =
|
||||
calib->dicrete_time_accel_noise_std().array().square().inverse();
|
||||
ccd.mocap_var_inv = pose_var_inv;
|
||||
}
|
||||
|
||||
void recompute_size() {
|
||||
offset_cam_intrinsics.clear();
|
||||
|
||||
size_t num_knots = spline.numKnots();
|
||||
|
||||
bias_block_offset = POSE_SIZE * num_knots;
|
||||
|
||||
size_t T_i_c_block_offset =
|
||||
bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE;
|
||||
|
||||
offset_T_i_c.emplace_back(T_i_c_block_offset);
|
||||
for (size_t i = 0; i < calib->T_i_c.size(); i++)
|
||||
offset_T_i_c.emplace_back(offset_T_i_c.back() + POSE_SIZE);
|
||||
|
||||
offset_cam_intrinsics.emplace_back(offset_T_i_c.back());
|
||||
for (size_t i = 0; i < calib->intrinsics.size(); i++)
|
||||
offset_cam_intrinsics.emplace_back(offset_cam_intrinsics.back() +
|
||||
calib->intrinsics[i].getN());
|
||||
|
||||
mocap_block_offset = offset_cam_intrinsics.back();
|
||||
|
||||
opt_size = mocap_block_offset + 2 * POSE_SIZE + 2;
|
||||
|
||||
// std::cerr << "bias_block_offset " << bias_block_offset << std::endl;
|
||||
// std::cerr << "mocap_block_offset " << mocap_block_offset << std::endl;
|
||||
// std::cerr << "opt_size " << opt_size << std::endl;
|
||||
// std::cerr << "offset_T_i_c.back() " << offset_T_i_c.back() <<
|
||||
// std::endl; std::cerr << "offset_cam_intrinsics.back() " <<
|
||||
// offset_cam_intrinsics.back()
|
||||
// << std::endl;
|
||||
}
|
||||
|
||||
// Returns true when converged
|
||||
bool optimize(bool use_intr, bool use_poses, bool use_april_corners,
|
||||
bool opt_cam_time_offset, bool opt_imu_scale, bool use_mocap,
|
||||
double huber_thresh, double stop_thresh, double& error,
|
||||
int& num_points, double& reprojection_error,
|
||||
bool print_info = true) {
|
||||
// std::cerr << "optimize num_knots " << num_knots << std::endl;
|
||||
|
||||
ccd.opt_intrinsics = use_intr;
|
||||
ccd.opt_cam_time_offset = opt_cam_time_offset;
|
||||
ccd.opt_imu_scale = opt_imu_scale;
|
||||
ccd.huber_thresh = huber_thresh;
|
||||
|
||||
LinearizeT lopt(opt_size, &spline, ccd);
|
||||
|
||||
// auto t1 = std::chrono::high_resolution_clock::now();
|
||||
|
||||
tbb::blocked_range<PoseDataIter> pose_range(pose_measurements.begin(),
|
||||
pose_measurements.end());
|
||||
tbb::blocked_range<AprilgridCornersDataIter> april_range(
|
||||
aprilgrid_corners_measurements.begin(),
|
||||
aprilgrid_corners_measurements.end());
|
||||
|
||||
tbb::blocked_range<MocapPoseDataIter> mocap_pose_range(
|
||||
mocap_measurements.begin(), mocap_measurements.end());
|
||||
|
||||
tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
|
||||
accel_measurements.end());
|
||||
|
||||
tbb::blocked_range<GyroDataIter> gyro_range(gyro_measurements.begin(),
|
||||
gyro_measurements.end());
|
||||
|
||||
if (use_poses) {
|
||||
tbb::parallel_reduce(pose_range, lopt);
|
||||
// lopt(pose_range);
|
||||
}
|
||||
|
||||
if (use_april_corners) {
|
||||
tbb::parallel_reduce(april_range, lopt);
|
||||
// lopt(april_range);
|
||||
}
|
||||
|
||||
if (use_mocap && mocap_initialized) {
|
||||
tbb::parallel_reduce(mocap_pose_range, lopt);
|
||||
// lopt(mocap_pose_range);
|
||||
} else if (use_mocap && !mocap_initialized) {
|
||||
std::cout << "Mocap residuals are not used. Initialize Mocap first!"
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
tbb::parallel_reduce(accel_range, lopt);
|
||||
tbb::parallel_reduce(gyro_range, lopt);
|
||||
|
||||
error = lopt.error;
|
||||
num_points = lopt.num_points;
|
||||
reprojection_error = lopt.reprojection_error;
|
||||
|
||||
if (print_info)
|
||||
std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
|
||||
<< lopt.num_points << std::endl;
|
||||
|
||||
lopt.accum.setup_solver();
|
||||
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
|
||||
|
||||
bool converged = false;
|
||||
bool step = false;
|
||||
int max_iter = 10;
|
||||
|
||||
while (!step && max_iter > 0 && !converged) {
|
||||
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
|
||||
for (int i = 0; i < Hdiag_lambda.size(); i++)
|
||||
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
|
||||
|
||||
VectorX inc_full = -lopt.accum.solve(&Hdiag_lambda);
|
||||
double max_inc = inc_full.array().abs().maxCoeff();
|
||||
|
||||
if (max_inc < stop_thresh) converged = true;
|
||||
|
||||
Calibration<Scalar> calib_backup = *calib;
|
||||
MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;
|
||||
SplineT spline_backup = spline;
|
||||
Vector3 g_backup = g;
|
||||
|
||||
applyInc(inc_full, offset_cam_intrinsics);
|
||||
|
||||
ComputeErrorSplineOpt eopt(opt_size, &spline, ccd);
|
||||
if (use_poses) {
|
||||
tbb::parallel_reduce(pose_range, eopt);
|
||||
}
|
||||
|
||||
if (use_april_corners) {
|
||||
tbb::parallel_reduce(april_range, eopt);
|
||||
}
|
||||
|
||||
if (use_mocap && mocap_initialized) {
|
||||
tbb::parallel_reduce(mocap_pose_range, eopt);
|
||||
} else if (use_mocap && !mocap_initialized) {
|
||||
std::cout << "Mocap residuals are not used. Initialize Mocap first!"
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
tbb::parallel_reduce(accel_range, eopt);
|
||||
tbb::parallel_reduce(gyro_range, eopt);
|
||||
|
||||
double f_diff = (lopt.error - eopt.error);
|
||||
double l_diff = 0.5 * inc_full.dot(inc_full * lambda - lopt.accum.getB());
|
||||
|
||||
// std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
|
||||
|
||||
double step_quality = f_diff / l_diff;
|
||||
|
||||
if (step_quality < 0) {
|
||||
if (print_info)
|
||||
std::cout << "\t[REJECTED] lambda:" << lambda
|
||||
<< " step_quality: " << step_quality
|
||||
<< " max_inc: " << max_inc << " Error: " << eopt.error
|
||||
<< " num points " << eopt.num_points << std::endl;
|
||||
lambda = std::min(max_lambda, lambda_vee * lambda);
|
||||
lambda_vee *= 2;
|
||||
|
||||
spline = spline_backup;
|
||||
*calib = calib_backup;
|
||||
*mocap_calib = mocap_calib_backup;
|
||||
g = g_backup;
|
||||
|
||||
} else {
|
||||
if (print_info)
|
||||
std::cout << "\t[ACCEPTED] lambda:" << lambda
|
||||
<< " step_quality: " << step_quality
|
||||
<< " max_inc: " << max_inc << " Error: " << eopt.error
|
||||
<< " num points " << eopt.num_points << std::endl;
|
||||
|
||||
lambda = std::max(
|
||||
min_lambda,
|
||||
lambda *
|
||||
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
|
||||
lambda_vee = 2;
|
||||
|
||||
error = eopt.error;
|
||||
num_points = eopt.num_points;
|
||||
reprojection_error = eopt.reprojection_error;
|
||||
|
||||
step = true;
|
||||
}
|
||||
max_iter--;
|
||||
}
|
||||
|
||||
if (converged && print_info) {
|
||||
std::cout << "[CONVERGED]" << std::endl;
|
||||
}
|
||||
|
||||
return converged;
|
||||
}
|
||||
|
||||
typename Calibration<Scalar>::Ptr calib;
|
||||
typename MocapCalibration<Scalar>::Ptr mocap_calib;
|
||||
bool mocap_initialized;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
|
||||
typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
|
||||
typedef
|
||||
typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
|
||||
typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
|
||||
AprilgridCornersDataIter;
|
||||
typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
|
||||
MocapPoseDataIter;
|
||||
|
||||
void applyInc(VectorX& inc_full,
|
||||
const std::vector<size_t>& offset_cam_intrinsics) {
|
||||
size_t num_knots = spline.numKnots();
|
||||
|
||||
for (size_t i = 0; i < num_knots; i++) {
|
||||
Vector6 inc = inc_full.template segment<POSE_SIZE>(POSE_SIZE * i);
|
||||
|
||||
// std::cerr << "i: " << i << " inc: " << inc.transpose() << std::endl;
|
||||
|
||||
spline.applyInc(i, inc);
|
||||
}
|
||||
|
||||
size_t bias_block_offset = POSE_SIZE * num_knots;
|
||||
calib->calib_accel_bias += inc_full.template segment<ACCEL_BIAS_SIZE>(
|
||||
bias_block_offset + ACCEL_BIAS_OFFSET);
|
||||
|
||||
calib->calib_gyro_bias += inc_full.template segment<GYRO_BIAS_SIZE>(
|
||||
bias_block_offset + GYRO_BIAS_OFFSET);
|
||||
g += inc_full.template segment<G_SIZE>(bias_block_offset + G_OFFSET);
|
||||
|
||||
size_t T_i_c_block_offset =
|
||||
bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE;
|
||||
for (size_t i = 0; i < calib->T_i_c.size(); i++) {
|
||||
calib->T_i_c[i] *= Sophus::se3_expd(inc_full.template segment<POSE_SIZE>(
|
||||
T_i_c_block_offset + i * POSE_SIZE));
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < calib->intrinsics.size(); i++) {
|
||||
calib->intrinsics[i].applyInc(inc_full.segment(
|
||||
offset_cam_intrinsics[i], calib->intrinsics[i].getN()));
|
||||
}
|
||||
|
||||
size_t mocap_block_offset = offset_cam_intrinsics.back();
|
||||
|
||||
mocap_calib->T_moc_w *= Sophus::se3_expd(
|
||||
inc_full.template segment<POSE_SIZE>(mocap_block_offset));
|
||||
mocap_calib->T_i_mark *= Sophus::se3_expd(
|
||||
inc_full.template segment<POSE_SIZE>(mocap_block_offset + POSE_SIZE));
|
||||
|
||||
mocap_calib->mocap_time_offset_ns +=
|
||||
1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE];
|
||||
|
||||
calib->cam_time_offset_ns +=
|
||||
1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE + 1];
|
||||
|
||||
// std::cout << "bias_block_offset " << bias_block_offset << std::endl;
|
||||
// std::cout << "mocap_block_offset " << mocap_block_offset << std::endl;
|
||||
}
|
||||
|
||||
Scalar lambda, min_lambda, max_lambda, lambda_vee;
|
||||
|
||||
int64_t min_time_us, max_time_us;
|
||||
|
||||
Eigen::aligned_deque<PoseData> pose_measurements;
|
||||
Eigen::aligned_deque<GyroData> gyro_measurements;
|
||||
Eigen::aligned_deque<AccelData> accel_measurements;
|
||||
Eigen::aligned_deque<AprilgridCornersData> aprilgrid_corners_measurements;
|
||||
Eigen::aligned_deque<MocapPoseData> mocap_measurements;
|
||||
|
||||
typename LinearizeT::CalibCommonData ccd;
|
||||
|
||||
std::vector<size_t> offset_cam_intrinsics;
|
||||
std::vector<size_t> offset_T_i_c;
|
||||
size_t mocap_block_offset;
|
||||
size_t bias_block_offset;
|
||||
size_t opt_size;
|
||||
|
||||
SplineT spline;
|
||||
Vector3 g;
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
|
||||
|
||||
int64_t dt_ns;
|
||||
}; // namespace basalt
|
||||
|
||||
} // namespace basalt
|
||||
146
include/basalt/utils/ba_utils.h
Normal file
146
include/basalt/utils/ba_utils.h
Normal file
@@ -0,0 +1,146 @@
|
||||
/**
|
||||
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>
|
||||
Sophus::SE3<Scalar> computeRelPose(
|
||||
const Sophus::SE3<Scalar>& T_w_i_h, const Sophus::SE3<Scalar>& T_i_c_h,
|
||||
const Sophus::SE3<Scalar>& T_w_i_t, const Sophus::SE3<Scalar>& T_i_c_t,
|
||||
Sophus::Matrix6<Scalar>* d_rel_d_h = nullptr,
|
||||
Sophus::Matrix6<Scalar>* d_rel_d_t = nullptr) {
|
||||
Sophus::SE3<Scalar> tmp2 = (T_i_c_t).inverse();
|
||||
|
||||
Sophus::SE3<Scalar> T_t_i_h_i;
|
||||
T_t_i_h_i.so3() = T_w_i_t.so3().inverse() * T_w_i_h.so3();
|
||||
T_t_i_h_i.translation() =
|
||||
T_w_i_t.so3().inverse() * (T_w_i_h.translation() - T_w_i_t.translation());
|
||||
|
||||
Sophus::SE3<Scalar> tmp = tmp2 * T_t_i_h_i;
|
||||
Sophus::SE3<Scalar> res = tmp * T_i_c_h;
|
||||
|
||||
if (d_rel_d_h) {
|
||||
Sophus::Matrix3<Scalar> R = T_w_i_h.so3().inverse().matrix();
|
||||
|
||||
Sophus::Matrix6<Scalar> RR;
|
||||
RR.setZero();
|
||||
RR.template topLeftCorner<3, 3>() = R;
|
||||
RR.template bottomRightCorner<3, 3>() = R;
|
||||
|
||||
*d_rel_d_h = tmp.Adj() * RR;
|
||||
}
|
||||
|
||||
if (d_rel_d_t) {
|
||||
Sophus::Matrix3<Scalar> R = T_w_i_t.so3().inverse().matrix();
|
||||
|
||||
Sophus::Matrix6<Scalar> RR;
|
||||
RR.setZero();
|
||||
RR.template topLeftCorner<3, 3>() = R;
|
||||
RR.template bottomRightCorner<3, 3>() = R;
|
||||
|
||||
*d_rel_d_t = -tmp2.Adj() * RR;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template <class Scalar, class CamT>
|
||||
inline bool linearizePoint(
|
||||
const Eigen::Matrix<Scalar, 2, 1>& kpt_obs, const Keypoint<Scalar>& kpt_pos,
|
||||
const Eigen::Matrix<Scalar, 4, 4>& T_t_h, const CamT& cam,
|
||||
Eigen::Matrix<Scalar, 2, 1>& res,
|
||||
Eigen::Matrix<Scalar, 2, POSE_SIZE>* d_res_d_xi = nullptr,
|
||||
Eigen::Matrix<Scalar, 2, 3>* d_res_d_p = nullptr,
|
||||
Eigen::Matrix<Scalar, 4, 1>* proj = nullptr) {
|
||||
static_assert(std::is_same_v<typename CamT::Scalar, Scalar>);
|
||||
|
||||
// Todo implement without jacobians
|
||||
Eigen::Matrix<Scalar, 4, 2> Jup;
|
||||
Eigen::Matrix<Scalar, 4, 1> p_h_3d;
|
||||
p_h_3d = StereographicParam<Scalar>::unproject(kpt_pos.direction, &Jup);
|
||||
p_h_3d[3] = kpt_pos.inv_dist;
|
||||
|
||||
const Eigen::Matrix<Scalar, 4, 1> p_t_3d = T_t_h * p_h_3d;
|
||||
|
||||
Eigen::Matrix<Scalar, 2, 4> Jp;
|
||||
bool valid = cam.project(p_t_3d, res, &Jp);
|
||||
valid &= res.array().isFinite().all();
|
||||
|
||||
if (!valid) {
|
||||
// std::cerr << " Invalid projection! kpt_pos.dir "
|
||||
// << kpt_pos.dir.transpose() << " kpt_pos.id " <<
|
||||
// kpt_pos.id
|
||||
// << " idx " << kpt_obs.kpt_id << std::endl;
|
||||
|
||||
// std::cerr << "T_t_h\n" << T_t_h << std::endl;
|
||||
// std::cerr << "p_h_3d\n" << p_h_3d.transpose() << std::endl;
|
||||
// std::cerr << "p_t_3d\n" << p_t_3d.transpose() << std::endl;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (proj) {
|
||||
proj->template head<2>() = res;
|
||||
(*proj)[2] = p_t_3d[3] / p_t_3d.template head<3>().norm();
|
||||
}
|
||||
res -= kpt_obs;
|
||||
|
||||
if (d_res_d_xi) {
|
||||
Eigen::Matrix<Scalar, 4, POSE_SIZE> d_point_d_xi;
|
||||
d_point_d_xi.template topLeftCorner<3, 3>() =
|
||||
Eigen::Matrix<Scalar, 3, 3>::Identity() * kpt_pos.inv_dist;
|
||||
d_point_d_xi.template topRightCorner<3, 3>() =
|
||||
-Sophus::SO3<Scalar>::hat(p_t_3d.template head<3>());
|
||||
d_point_d_xi.row(3).setZero();
|
||||
|
||||
*d_res_d_xi = Jp * d_point_d_xi;
|
||||
}
|
||||
|
||||
if (d_res_d_p) {
|
||||
Eigen::Matrix<Scalar, 4, 3> Jpp;
|
||||
Jpp.setZero();
|
||||
Jpp.template block<3, 2>(0, 0) = T_t_h.template topLeftCorner<3, 4>() * Jup;
|
||||
Jpp.col(2) = T_t_h.col(3);
|
||||
|
||||
*d_res_d_p = Jp * Jpp;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace basalt
|
||||
64
include/basalt/utils/cast_utils.hpp
Normal file
64
include/basalt/utils/cast_utils.hpp
Normal file
@@ -0,0 +1,64 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt.git
|
||||
|
||||
Copyright (c) 2021, 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 <type_traits>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class I>
|
||||
typename std::make_signed_t<I> signed_cast(I v) {
|
||||
static_assert(std::is_unsigned_v<I>, "no unsigned");
|
||||
return static_cast<typename std::make_signed_t<I>>(v);
|
||||
}
|
||||
|
||||
template <class I>
|
||||
typename std::make_unsigned_t<I> unsigned_cast(I v) {
|
||||
static_assert(std::is_signed_v<I>, "no signed");
|
||||
return static_cast<typename std::make_unsigned_t<I>>(v);
|
||||
}
|
||||
|
||||
// copy-assign map while casting Scalar type of values
|
||||
template <class M1, class M2>
|
||||
void assign_cast_map_values(M1& a, const M2& b) {
|
||||
using Scalar = typename M1::mapped_type::Scalar;
|
||||
a.clear();
|
||||
for (const auto& [k, v] : b) {
|
||||
a.try_emplace(k, v.template cast<Scalar>());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace basalt
|
||||
319
include/basalt/utils/common_types.h
Normal file
319
include/basalt/utils/common_types.h
Normal file
@@ -0,0 +1,319 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
Copyright (c) 2018, 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 <bitset>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <tbb/concurrent_unordered_map.h>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/StdVector>
|
||||
#include <sophus/se3.hpp>
|
||||
|
||||
#include <basalt/utils/hash.h>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
/// ids for 2D features detected in images
|
||||
using FeatureId = int;
|
||||
|
||||
/// identifies a frame of multiple images (stereo pair)
|
||||
using FrameId = int64_t;
|
||||
|
||||
/// identifies the camera (left or right)
|
||||
using CamId = std::size_t;
|
||||
|
||||
/// pair of image timestamp and camera id identifies an image (imageId)
|
||||
struct TimeCamId {
|
||||
TimeCamId() : frame_id(0), cam_id(0) {}
|
||||
|
||||
TimeCamId(const FrameId& frame_id, const CamId& cam_id)
|
||||
: frame_id(frame_id), cam_id(cam_id) {}
|
||||
|
||||
FrameId frame_id;
|
||||
CamId cam_id;
|
||||
};
|
||||
|
||||
inline std::ostream& operator<<(std::ostream& os, const TimeCamId& tcid) {
|
||||
os << tcid.frame_id << "_" << tcid.cam_id;
|
||||
return os;
|
||||
}
|
||||
|
||||
inline bool operator<(const TimeCamId& o1, const TimeCamId& o2) {
|
||||
if (o1.frame_id == o2.frame_id) return o1.cam_id < o2.cam_id;
|
||||
return o1.frame_id < o2.frame_id;
|
||||
}
|
||||
|
||||
inline bool operator==(const TimeCamId& o1, const TimeCamId& o2) {
|
||||
return o1.frame_id == o2.frame_id && o1.cam_id == o2.cam_id;
|
||||
}
|
||||
|
||||
inline bool operator!=(const TimeCamId& o1, const TimeCamId& o2) {
|
||||
return o1.frame_id != o2.frame_id || o1.cam_id != o2.cam_id;
|
||||
}
|
||||
|
||||
constexpr static const size_t FEATURE_HASH_MAX_SIZE = 32;
|
||||
using FeatureHash = std::bitset<FEATURE_HASH_MAX_SIZE>;
|
||||
using HashBowVector = std::vector<std::pair<FeatureHash, double>>;
|
||||
|
||||
/// keypoint positions and descriptors for an image
|
||||
struct KeypointsData {
|
||||
/// collection of 2d corner points (indexed by FeatureId)
|
||||
std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>
|
||||
corners;
|
||||
/// collection of feature orientation (in radian) with same index as `corners`
|
||||
/// (indexed by FeatureId)
|
||||
std::vector<double> corner_angles;
|
||||
/// collection of feature descriptors with same index as `corners` (indexed by
|
||||
/// FeatureId)
|
||||
std::vector<std::bitset<256>> corner_descriptors;
|
||||
|
||||
Eigen::aligned_vector<Eigen::Vector4d> corners_3d;
|
||||
|
||||
std::vector<FeatureHash> hashes;
|
||||
HashBowVector bow_vector;
|
||||
};
|
||||
|
||||
/// feature corners is a collection of { imageId => KeypointsData }
|
||||
using Corners = tbb::concurrent_unordered_map<TimeCamId, KeypointsData,
|
||||
std::hash<TimeCamId>>;
|
||||
|
||||
/// feature matches for an image pair
|
||||
struct MatchData {
|
||||
/// estimated transformation (based on inliers or calibration) from the second
|
||||
/// image's coordinate system to the first image's corrdinate system
|
||||
Sophus::SE3d T_i_j;
|
||||
/// collection of {featureId_i, featureId_j} pairs of all matches
|
||||
std::vector<std::pair<FeatureId, FeatureId>> matches;
|
||||
/// collection of {featureId_i, featureId_j} pairs of inlier matches
|
||||
std::vector<std::pair<FeatureId, FeatureId>> inliers;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
/// feature matches is a collection of { (imageId, imageId) => MatchData }
|
||||
using Matches = tbb::concurrent_unordered_map<
|
||||
std::pair<TimeCamId, TimeCamId>, MatchData,
|
||||
std::hash<std::pair<TimeCamId, TimeCamId>>,
|
||||
std::equal_to<std::pair<TimeCamId, TimeCamId>>,
|
||||
Eigen::aligned_allocator<
|
||||
std::pair<const std::pair<TimeCamId, TimeCamId>, MatchData>>>;
|
||||
|
||||
/// pair of image and feature indices
|
||||
using ImageFeaturePair = std::pair<TimeCamId, FeatureId>;
|
||||
|
||||
/// Feature tracks are collections of {ImageId => FeatureId}.
|
||||
/// I.e. a collection of all images that observed this feature and the
|
||||
/// corresponding feature index in that image.
|
||||
using FeatureTrack = std::map<TimeCamId, FeatureId>;
|
||||
|
||||
/// Ids for feature tracks; also used for landmarks created from (some of) the
|
||||
/// tracks;
|
||||
using TrackId = int64_t;
|
||||
|
||||
/// FeatureTracks is a collection {TrackId => FeatureTrack}
|
||||
using FeatureTracks = std::unordered_map<TrackId, FeatureTrack>;
|
||||
|
||||
/// cameras in the map
|
||||
struct Camera {
|
||||
// camera pose (transforms from camera to world)
|
||||
Sophus::SE3d T_w_c;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
/// landmarks in the map
|
||||
struct Landmark {
|
||||
/// 3d position in world coordinates
|
||||
Eigen::Vector3d p;
|
||||
|
||||
/// Inlier observations in the current map.
|
||||
/// This is a subset of the original feature track.
|
||||
FeatureTrack obs;
|
||||
|
||||
/// Outlier observations in the current map.
|
||||
/// This is a subset of the original feature track.
|
||||
FeatureTrack outlier_obs;
|
||||
};
|
||||
|
||||
/// collection {imageId => Camera} for all cameras in the map
|
||||
using Cameras =
|
||||
std::map<TimeCamId, Camera, std::less<TimeCamId>,
|
||||
Eigen::aligned_allocator<std::pair<const TimeCamId, Camera>>>;
|
||||
|
||||
/// collection {trackId => Landmark} for all landmarks in the map.
|
||||
/// trackIds correspond to feature_tracks
|
||||
using Landmarks = std::unordered_map<TrackId, Landmark>;
|
||||
|
||||
/// camera candidate to be added to map
|
||||
struct CameraCandidate {
|
||||
TimeCamId tcid;
|
||||
std::vector<TrackId> shared_tracks;
|
||||
|
||||
// keep track of different stages of adding a set of candidate cameras and its
|
||||
// landmarks to the map
|
||||
bool tried = false; //!< tried to add to map
|
||||
bool camera_added = false; //!< succeeded to add to map
|
||||
bool landmarks_added = false; //!< added new landmarks to map
|
||||
};
|
||||
|
||||
/// list of current candidates and some book keeping for the different stages
|
||||
struct CameraCandidates {
|
||||
enum Stage {
|
||||
ComputeCandidates,
|
||||
AddCameras,
|
||||
AddLandmarks,
|
||||
Optimize,
|
||||
RemoveOutliers,
|
||||
Done
|
||||
};
|
||||
|
||||
std::vector<CameraCandidate> cameras;
|
||||
Stage current_stage = ComputeCandidates;
|
||||
int min_localization_inliers = 0;
|
||||
int max_cameras_to_add = 0;
|
||||
|
||||
int num_cameras_added() {
|
||||
int num_added = 0;
|
||||
for (const auto& c : cameras) {
|
||||
if (c.camera_added) {
|
||||
++num_added;
|
||||
}
|
||||
}
|
||||
return num_added;
|
||||
}
|
||||
|
||||
int num_landmarks_added() {
|
||||
int num_added = 0;
|
||||
for (const auto& c : cameras) {
|
||||
if (c.landmarks_added) {
|
||||
++num_added;
|
||||
}
|
||||
}
|
||||
return num_added;
|
||||
}
|
||||
};
|
||||
|
||||
/// Flags for different landmark outlier criteria
|
||||
enum OutlierFlags {
|
||||
OutlierNone = 0,
|
||||
// reprojection error much too large
|
||||
OutlierReprojectionErrorHuge = 1 << 0,
|
||||
// reprojection error too large
|
||||
OutlierReprojectionErrorNormal = 1 << 1,
|
||||
// distance to a camera too small
|
||||
OutlierCameraDistance = 1 << 2,
|
||||
// z-coord in some camera frame too small
|
||||
OutlierZCoordinate = 1 << 3
|
||||
};
|
||||
|
||||
/// info on a single projected landmark
|
||||
struct ProjectedLandmark {
|
||||
Eigen::Vector2d point_measured; //!< detected feature location
|
||||
Eigen::Vector2d point_reprojected; //!< landmark projected into image
|
||||
Eigen::Vector3d point_3d_c; //!< 3d point in camera coordinates
|
||||
TrackId track_id = -1; //!< corresponding track_id
|
||||
double reprojection_error = 0; //!< current reprojection error
|
||||
unsigned int outlier_flags = OutlierNone; //!< flags for outlier
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
using ProjectedLandmarkPtr = std::shared_ptr<ProjectedLandmark>;
|
||||
using ProjectedLandmarkConstPtr = std::shared_ptr<const ProjectedLandmark>;
|
||||
|
||||
/// all landmark projections for inlier and outlier observations for a single
|
||||
/// image
|
||||
struct ImageProjection {
|
||||
std::vector<ProjectedLandmarkConstPtr> obs;
|
||||
std::vector<ProjectedLandmarkConstPtr> outlier_obs;
|
||||
};
|
||||
|
||||
/// projections for all images
|
||||
using ImageProjections = std::map<TimeCamId, ImageProjection>;
|
||||
|
||||
/// inlier projections indexed per track
|
||||
using TrackProjections =
|
||||
std::unordered_map<TrackId, std::map<TimeCamId, ProjectedLandmarkConstPtr>>;
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
namespace cereal {
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::TimeCamId& c) {
|
||||
ar(c.frame_id, c.cam_id);
|
||||
}
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::KeypointsData& c) {
|
||||
ar(c.corners, c.corner_angles, c.corner_descriptors);
|
||||
}
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::MatchData& c) {
|
||||
ar(c.T_i_j, c.matches, c.inliers);
|
||||
}
|
||||
|
||||
} // namespace cereal
|
||||
|
||||
namespace std {
|
||||
|
||||
template <>
|
||||
struct hash<basalt::TimeCamId> {
|
||||
size_t operator()(const basalt::TimeCamId& x) const {
|
||||
size_t seed = 0;
|
||||
basalt::hash_combine(seed, x.frame_id);
|
||||
basalt::hash_combine(seed, x.cam_id);
|
||||
return seed;
|
||||
}
|
||||
};
|
||||
|
||||
template <>
|
||||
struct hash<std::pair<basalt::TimeCamId, basalt::TimeCamId>> {
|
||||
size_t operator()(
|
||||
const std::pair<basalt::TimeCamId, basalt::TimeCamId>& x) const {
|
||||
size_t seed = 0;
|
||||
basalt::hash_combine(seed, x.first.frame_id);
|
||||
basalt::hash_combine(seed, x.first.cam_id);
|
||||
basalt::hash_combine(seed, x.second.frame_id);
|
||||
basalt::hash_combine(seed, x.second.cam_id);
|
||||
return seed;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace std
|
||||
70
include/basalt/utils/filesystem.h
Normal file
70
include/basalt/utils/filesystem.h
Normal file
@@ -0,0 +1,70 @@
|
||||
#pragma once
|
||||
|
||||
// Check for feature test macro for <filesystem>
|
||||
#if defined(__cpp_lib_filesystem)
|
||||
#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0
|
||||
|
||||
// Check for feature test macro for <experimental/filesystem>
|
||||
#elif defined(__cpp_lib_experimental_filesystem)
|
||||
#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
|
||||
|
||||
// We can't check if headers exist...
|
||||
// Let's assume experimental to be safe
|
||||
#elif !defined(__has_include)
|
||||
#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
|
||||
|
||||
// Check if the header "<filesystem>" exists
|
||||
#elif __has_include(<filesystem>)
|
||||
|
||||
// If we're compiling on Visual Studio and are not compiling with C++17, we need
|
||||
// to use experimental
|
||||
#ifdef _MSC_VER
|
||||
|
||||
// Check and include header that defines "_HAS_CXX17"
|
||||
#if __has_include(<yvals_core.h>)
|
||||
#include <yvals_core.h>
|
||||
|
||||
// Check for enabled C++17 support
|
||||
#if defined(_HAS_CXX17) && _HAS_CXX17
|
||||
// We're using C++17, so let's use the normal version
|
||||
#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// If the marco isn't defined yet, that means any of the other VS specific
|
||||
// checks failed, so we need to use experimental
|
||||
#ifndef INCLUDE_STD_FILESYSTEM_EXPERIMENTAL
|
||||
#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
|
||||
#endif
|
||||
|
||||
// Not on Visual Studio. Let's use the normal version
|
||||
#else // #ifdef _MSC_VER
|
||||
#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0
|
||||
#endif
|
||||
|
||||
// Check if the header "<filesystem>" exists
|
||||
#elif __has_include(<experimental/filesystem>)
|
||||
#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
|
||||
|
||||
// Fail if neither header is available with a nice error message
|
||||
#else
|
||||
#error Could not find system header "<filesystem>" or "<experimental/filesystem>"
|
||||
#endif
|
||||
|
||||
// We priously determined that we need the exprimental version
|
||||
#if INCLUDE_STD_FILESYSTEM_EXPERIMENTAL
|
||||
#include <experimental/filesystem>
|
||||
|
||||
namespace basalt {
|
||||
namespace fs = std::experimental::filesystem;
|
||||
}
|
||||
|
||||
// We have a decent compiler and can use the normal version
|
||||
#else
|
||||
#include <filesystem>
|
||||
|
||||
namespace basalt {
|
||||
namespace fs = std::filesystem;
|
||||
}
|
||||
|
||||
#endif
|
||||
385
include/basalt/utils/imu_types.h
Normal file
385
include/basalt/utils/imu_types.h
Normal file
@@ -0,0 +1,385 @@
|
||||
/**
|
||||
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 <map>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <sophus/se3.hpp>
|
||||
|
||||
#include <basalt/imu/imu_types.h>
|
||||
#include <basalt/optical_flow/optical_flow.h>
|
||||
#include <basalt/utils/common_types.h>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
#include <cereal/cereal.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Scalar_>
|
||||
class IntegratedImuMeasurement;
|
||||
|
||||
template <class Scalar_>
|
||||
struct PoseStateWithLin;
|
||||
|
||||
namespace constants {
|
||||
static const Eigen::Vector3d g(0, 0, -9.81);
|
||||
static const Eigen::Vector3d g_dir(0, 0, -1);
|
||||
} // namespace constants
|
||||
|
||||
template <class Scalar_>
|
||||
struct PoseVelBiasStateWithLin {
|
||||
using Scalar = Scalar_;
|
||||
|
||||
using VecN = typename PoseVelBiasState<Scalar>::VecN;
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using SE3 = Sophus::SE3<Scalar>;
|
||||
|
||||
PoseVelBiasStateWithLin() {
|
||||
linearized = false;
|
||||
delta.setZero();
|
||||
};
|
||||
|
||||
PoseVelBiasStateWithLin(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i,
|
||||
const Vec3& bias_gyro, const Vec3& bias_accel,
|
||||
bool linearized)
|
||||
: linearized(linearized),
|
||||
state_linearized(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel) {
|
||||
delta.setZero();
|
||||
state_current = state_linearized;
|
||||
}
|
||||
|
||||
explicit PoseVelBiasStateWithLin(const PoseVelBiasState<Scalar>& other)
|
||||
: linearized(false), state_linearized(other) {
|
||||
delta.setZero();
|
||||
state_current = other;
|
||||
}
|
||||
|
||||
template <class Scalar2>
|
||||
PoseVelBiasStateWithLin<Scalar2> cast() const {
|
||||
PoseVelBiasStateWithLin<Scalar2> a;
|
||||
a.linearized = linearized;
|
||||
a.delta = delta.template cast<Scalar2>();
|
||||
a.state_linearized = state_linearized.template cast<Scalar2>();
|
||||
a.state_current = state_current.template cast<Scalar2>();
|
||||
return a;
|
||||
}
|
||||
|
||||
void setLinFalse() {
|
||||
linearized = false;
|
||||
delta.setZero();
|
||||
}
|
||||
|
||||
void setLinTrue() {
|
||||
linearized = true;
|
||||
BASALT_ASSERT(delta.isApproxToConstant(0));
|
||||
state_current = state_linearized;
|
||||
}
|
||||
|
||||
void applyInc(const VecN& inc) {
|
||||
if (!linearized) {
|
||||
state_linearized.applyInc(inc);
|
||||
} else {
|
||||
delta += inc;
|
||||
state_current = state_linearized;
|
||||
state_current.applyInc(delta);
|
||||
}
|
||||
}
|
||||
|
||||
inline const PoseVelBiasState<Scalar>& getState() const {
|
||||
if (!linearized) {
|
||||
return state_linearized;
|
||||
} else {
|
||||
return state_current;
|
||||
}
|
||||
}
|
||||
|
||||
inline const PoseVelBiasState<Scalar>& getStateLin() const {
|
||||
return state_linearized;
|
||||
}
|
||||
|
||||
inline bool isLinearized() const { return linearized; }
|
||||
inline const VecN& getDelta() const { return delta; }
|
||||
inline int64_t getT_ns() const { return state_linearized.t_ns; }
|
||||
|
||||
inline void backup() {
|
||||
backup_delta = delta;
|
||||
backup_state_linearized = state_linearized;
|
||||
backup_state_current = state_current;
|
||||
}
|
||||
|
||||
inline void restore() {
|
||||
delta = backup_delta;
|
||||
state_linearized = backup_state_linearized;
|
||||
state_current = backup_state_current;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
bool linearized;
|
||||
VecN delta;
|
||||
PoseVelBiasState<Scalar> state_linearized, state_current;
|
||||
|
||||
VecN backup_delta;
|
||||
PoseVelBiasState<Scalar> backup_state_linearized, backup_state_current;
|
||||
|
||||
// give access to private members for constructor of PoseStateWithLin
|
||||
friend struct PoseStateWithLin<Scalar>;
|
||||
|
||||
// give access to private members for cast() implementation
|
||||
template <class>
|
||||
friend struct PoseVelBiasStateWithLin;
|
||||
|
||||
friend class cereal::access;
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar) {
|
||||
ar(state_linearized.T_w_i);
|
||||
ar(state_linearized.vel_w_i);
|
||||
ar(state_linearized.bias_gyro);
|
||||
ar(state_linearized.bias_accel);
|
||||
ar(state_current.T_w_i);
|
||||
ar(state_current.vel_w_i);
|
||||
ar(state_current.bias_gyro);
|
||||
ar(state_current.bias_accel);
|
||||
ar(delta);
|
||||
ar(linearized);
|
||||
ar(state_linearized.t_ns);
|
||||
}
|
||||
};
|
||||
|
||||
template <class Scalar_>
|
||||
struct PoseStateWithLin {
|
||||
using Scalar = Scalar_;
|
||||
using VecN = typename PoseState<Scalar>::VecN;
|
||||
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
|
||||
using SE3 = Sophus::SE3<Scalar>;
|
||||
|
||||
PoseStateWithLin() {
|
||||
linearized = false;
|
||||
delta.setZero();
|
||||
};
|
||||
|
||||
PoseStateWithLin(int64_t t_ns, const SE3& T_w_i, bool linearized = false)
|
||||
: linearized(linearized), pose_linearized(t_ns, T_w_i) {
|
||||
delta.setZero();
|
||||
T_w_i_current = T_w_i;
|
||||
}
|
||||
|
||||
explicit PoseStateWithLin(const PoseVelBiasStateWithLin<Scalar>& other)
|
||||
: linearized(other.linearized),
|
||||
delta(other.delta.template head<6>()),
|
||||
pose_linearized(other.state_linearized.t_ns,
|
||||
other.state_linearized.T_w_i) {
|
||||
T_w_i_current = pose_linearized.T_w_i;
|
||||
PoseState<Scalar>::incPose(delta, T_w_i_current);
|
||||
}
|
||||
|
||||
template <class Scalar2>
|
||||
PoseStateWithLin<Scalar2> cast() const {
|
||||
PoseStateWithLin<Scalar2> a;
|
||||
a.linearized = linearized;
|
||||
a.delta = delta.template cast<Scalar2>();
|
||||
a.pose_linearized = pose_linearized.template cast<Scalar2>();
|
||||
a.T_w_i_current = T_w_i_current.template cast<Scalar2>();
|
||||
return a;
|
||||
}
|
||||
|
||||
void setLinTrue() {
|
||||
linearized = true;
|
||||
BASALT_ASSERT(delta.isApproxToConstant(0));
|
||||
T_w_i_current = pose_linearized.T_w_i;
|
||||
}
|
||||
|
||||
inline const SE3& getPose() const {
|
||||
if (!linearized) {
|
||||
return pose_linearized.T_w_i;
|
||||
} else {
|
||||
return T_w_i_current;
|
||||
}
|
||||
}
|
||||
|
||||
inline const SE3& getPoseLin() const { return pose_linearized.T_w_i; }
|
||||
|
||||
inline void applyInc(const VecN& inc) {
|
||||
if (!linearized) {
|
||||
PoseState<Scalar>::incPose(inc, pose_linearized.T_w_i);
|
||||
} else {
|
||||
delta += inc;
|
||||
T_w_i_current = pose_linearized.T_w_i;
|
||||
PoseState<Scalar>::incPose(delta, T_w_i_current);
|
||||
}
|
||||
}
|
||||
|
||||
inline bool isLinearized() const { return linearized; }
|
||||
inline const VecN& getDelta() const { return delta; }
|
||||
inline int64_t getT_ns() const { return pose_linearized.t_ns; }
|
||||
|
||||
inline void backup() {
|
||||
backup_delta = delta;
|
||||
backup_pose_linearized = pose_linearized;
|
||||
backup_T_w_i_current = T_w_i_current;
|
||||
}
|
||||
|
||||
inline void restore() {
|
||||
delta = backup_delta;
|
||||
pose_linearized = backup_pose_linearized;
|
||||
T_w_i_current = backup_T_w_i_current;
|
||||
}
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
private:
|
||||
bool linearized;
|
||||
VecN delta;
|
||||
PoseState<Scalar> pose_linearized;
|
||||
SE3 T_w_i_current;
|
||||
|
||||
VecN backup_delta;
|
||||
PoseState<Scalar> backup_pose_linearized;
|
||||
SE3 backup_T_w_i_current;
|
||||
|
||||
// give access to private members for cast() implementation
|
||||
template <class>
|
||||
friend struct PoseStateWithLin;
|
||||
|
||||
friend class cereal::access;
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar) {
|
||||
ar(pose_linearized.T_w_i);
|
||||
ar(T_w_i_current);
|
||||
ar(delta);
|
||||
ar(linearized);
|
||||
ar(pose_linearized.t_ns);
|
||||
}
|
||||
};
|
||||
|
||||
struct AbsOrderMap {
|
||||
std::map<int64_t, std::pair<int, int>> abs_order_map;
|
||||
size_t items = 0;
|
||||
size_t total_size = 0;
|
||||
|
||||
void print_order() {
|
||||
for (const auto& kv : abs_order_map) {
|
||||
std::cout << kv.first << " (" << kv.second.first << ","
|
||||
<< kv.second.second << ")" << std::endl;
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
template <class Scalar_>
|
||||
struct ImuLinData {
|
||||
using Scalar = Scalar_;
|
||||
|
||||
const Eigen::Matrix<Scalar, 3, 1>& g;
|
||||
const Eigen::Matrix<Scalar, 3, 1>& gyro_bias_weight_sqrt;
|
||||
const Eigen::Matrix<Scalar, 3, 1>& accel_bias_weight_sqrt;
|
||||
|
||||
std::map<int64_t, const IntegratedImuMeasurement<Scalar>*> imu_meas;
|
||||
};
|
||||
|
||||
template <class Scalar_>
|
||||
struct MargLinData {
|
||||
using Scalar = Scalar_;
|
||||
|
||||
bool is_sqrt = true;
|
||||
|
||||
AbsOrderMap order;
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> H;
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> b;
|
||||
};
|
||||
|
||||
struct MargData {
|
||||
typedef std::shared_ptr<MargData> Ptr;
|
||||
|
||||
AbsOrderMap aom;
|
||||
Eigen::MatrixXd abs_H;
|
||||
Eigen::VectorXd abs_b;
|
||||
Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>> frame_states;
|
||||
Eigen::aligned_map<int64_t, PoseStateWithLin<double>> frame_poses;
|
||||
std::set<int64_t> kfs_all;
|
||||
std::set<int64_t> kfs_to_marg;
|
||||
bool use_imu;
|
||||
|
||||
std::vector<OpticalFlowResult::Ptr> opt_flow_res;
|
||||
};
|
||||
|
||||
struct RelPoseFactor {
|
||||
int64_t t_i_ns, t_j_ns;
|
||||
|
||||
Sophus::SE3d T_i_j;
|
||||
Sophus::Matrix6d cov_inv;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
struct RollPitchFactor {
|
||||
int64_t t_ns;
|
||||
|
||||
Sophus::SO3d R_w_i_meas;
|
||||
|
||||
Eigen::Matrix2d cov_inv;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
} // namespace basalt
|
||||
|
||||
namespace cereal {
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::AbsOrderMap& a) {
|
||||
ar(a.total_size);
|
||||
ar(a.items);
|
||||
ar(a.abs_order_map);
|
||||
}
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::MargData& m) {
|
||||
ar(m.aom);
|
||||
ar(m.abs_H);
|
||||
ar(m.abs_b);
|
||||
ar(m.frame_poses);
|
||||
ar(m.frame_states);
|
||||
ar(m.kfs_all);
|
||||
ar(m.kfs_to_marg);
|
||||
ar(m.use_imu);
|
||||
}
|
||||
|
||||
} // namespace cereal
|
||||
104
include/basalt/utils/keypoints.h
Normal file
104
include/basalt/utils/keypoints.h
Normal file
@@ -0,0 +1,104 @@
|
||||
/**
|
||||
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 <bitset>
|
||||
#include <set>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <sophus/se3.hpp>
|
||||
|
||||
#include <basalt/image/image.h>
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
#include <basalt/utils/common_types.h>
|
||||
#include <basalt/camera/generic_camera.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
typedef std::bitset<256> Descriptor;
|
||||
|
||||
void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
|
||||
KeypointsData& kd, int num_features);
|
||||
|
||||
void detectKeypoints(
|
||||
const basalt::Image<const uint16_t>& img_raw, KeypointsData& kd,
|
||||
int PATCH_SIZE = 32, int num_points_cell = 1,
|
||||
const Eigen::aligned_vector<Eigen::Vector2d>& current_points =
|
||||
Eigen::aligned_vector<Eigen::Vector2d>());
|
||||
|
||||
void computeAngles(const basalt::Image<const uint16_t>& img_raw,
|
||||
KeypointsData& kd, bool rotate_features);
|
||||
|
||||
void computeDescriptors(const basalt::Image<const uint16_t>& img_raw,
|
||||
KeypointsData& kd);
|
||||
|
||||
void matchDescriptors(const std::vector<std::bitset<256>>& corner_descriptors_1,
|
||||
const std::vector<std::bitset<256>>& corner_descriptors_2,
|
||||
std::vector<std::pair<int, int>>& matches, int threshold,
|
||||
double dist_2_best);
|
||||
|
||||
inline void computeEssential(const Sophus::SE3d& T_0_1, Eigen::Matrix4d& E) {
|
||||
E.setZero();
|
||||
const Eigen::Vector3d t_0_1 = T_0_1.translation();
|
||||
const Eigen::Matrix3d R_0_1 = T_0_1.rotationMatrix();
|
||||
|
||||
E.topLeftCorner<3, 3>() = Sophus::SO3d::hat(t_0_1.normalized()) * R_0_1;
|
||||
}
|
||||
|
||||
inline void findInliersEssential(const KeypointsData& kd1,
|
||||
const KeypointsData& kd2,
|
||||
const Eigen::Matrix4d& E,
|
||||
double epipolar_error_threshold,
|
||||
MatchData& md) {
|
||||
md.inliers.clear();
|
||||
|
||||
for (size_t j = 0; j < md.matches.size(); j++) {
|
||||
const Eigen::Vector4d p0_3d = kd1.corners_3d[md.matches[j].first];
|
||||
const Eigen::Vector4d p1_3d = kd2.corners_3d[md.matches[j].second];
|
||||
|
||||
const double epipolar_error = std::abs(p0_3d.transpose() * E * p1_3d);
|
||||
|
||||
if (epipolar_error < epipolar_error_threshold) {
|
||||
md.inliers.push_back(md.matches[j]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void findInliersRansac(const KeypointsData& kd1, const KeypointsData& kd2,
|
||||
const double ransac_thresh, const int ransac_min_inliers,
|
||||
MatchData& md);
|
||||
|
||||
} // namespace basalt
|
||||
120
include/basalt/utils/nfr.h
Normal file
120
include/basalt/utils/nfr.h
Normal file
@@ -0,0 +1,120 @@
|
||||
/**
|
||||
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 <sophus/se3.hpp>
|
||||
|
||||
#include <basalt/utils/imu_types.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
inline Sophus::Vector6d relPoseError(
|
||||
const Sophus::SE3d& T_i_j, const Sophus::SE3d& T_w_i,
|
||||
const Sophus::SE3d& T_w_j, Sophus::Matrix6d* d_res_d_T_w_i = nullptr,
|
||||
Sophus::Matrix6d* d_res_d_T_w_j = nullptr) {
|
||||
Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i;
|
||||
Sophus::Vector6d res = Sophus::se3_logd(T_i_j * T_j_i);
|
||||
|
||||
if (d_res_d_T_w_i || d_res_d_T_w_j) {
|
||||
Sophus::Matrix6d J;
|
||||
Sophus::rightJacobianInvSE3Decoupled(res, J);
|
||||
|
||||
Eigen::Matrix3d R = T_w_i.so3().inverse().matrix();
|
||||
|
||||
Sophus::Matrix6d Adj;
|
||||
Adj.setZero();
|
||||
Adj.topLeftCorner<3, 3>() = R;
|
||||
Adj.bottomRightCorner<3, 3>() = R;
|
||||
|
||||
if (d_res_d_T_w_i) {
|
||||
*d_res_d_T_w_i = J * Adj;
|
||||
}
|
||||
|
||||
if (d_res_d_T_w_j) {
|
||||
Adj.topRightCorner<3, 3>() =
|
||||
Sophus::SO3d::hat(T_j_i.inverse().translation()) * R;
|
||||
*d_res_d_T_w_j = -J * Adj;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
inline Sophus::Vector3d absPositionError(
|
||||
const Sophus::SE3d& T_w_i, const Eigen::Vector3d pos,
|
||||
Eigen::Matrix<double, 3, 6>* d_res_d_T_w_i = nullptr) {
|
||||
if (d_res_d_T_w_i) {
|
||||
d_res_d_T_w_i->topLeftCorner<3, 3>().setIdentity();
|
||||
d_res_d_T_w_i->topRightCorner<3, 3>().setZero();
|
||||
}
|
||||
|
||||
return T_w_i.translation() - pos;
|
||||
}
|
||||
|
||||
inline double yawError(const Sophus::SE3d& T_w_i,
|
||||
const Eigen::Vector3d yaw_dir_body,
|
||||
Eigen::Matrix<double, 1, 6>* d_res_d_T_w_i = nullptr) {
|
||||
Eigen::Matrix3d curr_R_w_i = T_w_i.so3().matrix();
|
||||
Eigen::Vector3d tmp = curr_R_w_i * yaw_dir_body;
|
||||
double res_yaw = tmp[1];
|
||||
|
||||
if (d_res_d_T_w_i) {
|
||||
d_res_d_T_w_i->setZero();
|
||||
(*d_res_d_T_w_i)[3] = -tmp[2];
|
||||
(*d_res_d_T_w_i)[5] = tmp[0];
|
||||
}
|
||||
|
||||
return res_yaw;
|
||||
}
|
||||
|
||||
inline Sophus::Vector2d rollPitchError(
|
||||
const Sophus::SE3d& T_w_i, const Sophus::SO3d& R_w_i_meas,
|
||||
Eigen::Matrix<double, 2, 6>* d_res_d_T_w_i = nullptr) {
|
||||
// Assumes g direction is negative Z in world coordinate frame
|
||||
|
||||
Eigen::Matrix3d R = (R_w_i_meas * T_w_i.so3().inverse()).matrix();
|
||||
Eigen::Vector3d res = R * (-Eigen::Vector3d::UnitZ());
|
||||
|
||||
if (d_res_d_T_w_i) {
|
||||
d_res_d_T_w_i->setZero();
|
||||
(*d_res_d_T_w_i)(0, 3) = -R(0, 1);
|
||||
(*d_res_d_T_w_i)(1, 3) = -R(1, 1);
|
||||
(*d_res_d_T_w_i)(0, 4) = R(0, 0);
|
||||
(*d_res_d_T_w_i)(1, 4) = R(1, 0);
|
||||
}
|
||||
|
||||
return res.head<2>();
|
||||
}
|
||||
} // namespace basalt
|
||||
53
include/basalt/utils/sim_utils.h
Normal file
53
include/basalt/utils/sim_utils.h
Normal file
@@ -0,0 +1,53 @@
|
||||
/**
|
||||
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/sophus_utils.hpp>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
struct SimObservations {
|
||||
Eigen::aligned_vector<Eigen::Vector2d> pos;
|
||||
std::vector<int> id;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
namespace cereal {
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, basalt::SimObservations& c) {
|
||||
ar(c.pos, c.id);
|
||||
}
|
||||
} // namespace cereal
|
||||
60
include/basalt/utils/system_utils.h
Normal file
60
include/basalt/utils/system_utils.h
Normal file
@@ -0,0 +1,60 @@
|
||||
/**
|
||||
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_SYSTEM_UTILS_H
|
||||
#define BASALT_SYSTEM_UTILS_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
inline std::string ensure_trailing_slash(const std::string& path) {
|
||||
if (!path.empty() && path[path.size() - 1] != '/') {
|
||||
return path + "/";
|
||||
} else {
|
||||
return path;
|
||||
}
|
||||
}
|
||||
|
||||
struct MemoryInfo {
|
||||
uint64_t resident_memory = 0; //!< in bytes
|
||||
uint64_t resident_memory_peak = 0; //!< in bytes
|
||||
};
|
||||
|
||||
bool get_memory_info(MemoryInfo& info);
|
||||
|
||||
} // namespace basalt
|
||||
|
||||
#endif // include guard
|
||||
77
include/basalt/utils/test_utils.h
Normal file
77
include/basalt/utils/test_utils.h
Normal file
@@ -0,0 +1,77 @@
|
||||
/**
|
||||
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_TEST_UTILS_H
|
||||
#define BASALT_TEST_UTILS_H
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
template <typename Derived1, typename Derived2, typename F>
|
||||
void test_jacobian_code(const std::string& name,
|
||||
const Eigen::MatrixBase<Derived1>& Ja, F func,
|
||||
const Eigen::MatrixBase<Derived2>& x0,
|
||||
double eps = 1e-8, double max_norm = 1e-4) {
|
||||
typedef typename Derived1::Scalar Scalar;
|
||||
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Jn = Ja;
|
||||
Jn.setZero();
|
||||
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> inc = x0;
|
||||
for (int i = 0; i < Jn.cols(); i++) {
|
||||
inc.setZero();
|
||||
inc[i] += eps;
|
||||
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fpe = func(x0 + inc);
|
||||
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fme = func(x0 - inc);
|
||||
|
||||
Jn.col(i) = (fpe - fme) / (2 * eps);
|
||||
}
|
||||
|
||||
Scalar diff = (Ja - Jn).norm();
|
||||
|
||||
if (diff > max_norm || !Ja.allFinite()) {
|
||||
std::cerr << name << std::endl;
|
||||
std::cerr << "Numeric Jacobian is different from analytic. Norm difference "
|
||||
<< diff << std::endl;
|
||||
std::cerr << "Ja\n" << Ja << std::endl;
|
||||
std::cerr << "Jn\n" << Jn << std::endl;
|
||||
} else {
|
||||
// std::cout << name << std::endl;
|
||||
// std::cout << "Success" << std::endl;
|
||||
// std::cout << "Ja\n" << Ja << std::endl;
|
||||
// std::cout << "Jn\n" << Jn << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // BASALT_TEST_UTILS_H
|
||||
141
include/basalt/utils/time_utils.hpp
Normal file
141
include/basalt/utils/time_utils.hpp
Normal file
@@ -0,0 +1,141 @@
|
||||
/**
|
||||
BSD 3-Clause License
|
||||
|
||||
This file is part of the Basalt project.
|
||||
https://gitlab.com/VladyslavUsenko/basalt.git
|
||||
|
||||
Copyright (c) 2020-2021, 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 <chrono>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <variant>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
template <class Clock = std::chrono::high_resolution_clock>
|
||||
class Timer {
|
||||
public:
|
||||
/// start timer
|
||||
Timer() : start_(Clock::now()) {}
|
||||
|
||||
/// return elapsed time in seconds
|
||||
double elapsed() const {
|
||||
return std::chrono::duration<double>(Clock::now() - start_).count();
|
||||
}
|
||||
|
||||
/// return elapsed time in seconds and reset timer
|
||||
double reset() {
|
||||
const auto now = Clock::now();
|
||||
const double elapsed = std::chrono::duration<double>(now - start_).count();
|
||||
start_ = now;
|
||||
return elapsed;
|
||||
}
|
||||
|
||||
private:
|
||||
std::chrono::time_point<Clock> start_;
|
||||
};
|
||||
|
||||
template <class Scalar = double>
|
||||
struct assign_op {
|
||||
void operator()(Scalar& lhs, const Scalar& rhs) { lhs = rhs; }
|
||||
};
|
||||
|
||||
template <class Scalar = double>
|
||||
struct plus_assign_op {
|
||||
void operator()(Scalar& lhs, const Scalar& rhs) { lhs += rhs; }
|
||||
};
|
||||
|
||||
template <class T = Timer<>, class Assign = assign_op<>>
|
||||
class ScopedTimer {
|
||||
public:
|
||||
explicit ScopedTimer(double& dest) : dest_(dest) {}
|
||||
~ScopedTimer() { Assign()(dest_, timer_.elapsed()); }
|
||||
|
||||
private:
|
||||
double& dest_;
|
||||
T timer_;
|
||||
};
|
||||
|
||||
using ScopedTimerAdd = ScopedTimer<Timer<>, plus_assign_op<>>;
|
||||
|
||||
template <class F>
|
||||
void log_timing(double& time, F fun) {
|
||||
Timer timer;
|
||||
fun();
|
||||
time = timer.elapsed();
|
||||
}
|
||||
|
||||
template <class F>
|
||||
void log_timing_add(double& time, F fun) {
|
||||
Timer timer;
|
||||
fun();
|
||||
time += timer.elapsed();
|
||||
}
|
||||
|
||||
class ExecutionStats {
|
||||
public:
|
||||
struct Meta {
|
||||
inline Meta& format(const std::string& s) {
|
||||
format_ = s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
// overwrite the meta data from another object
|
||||
void set_meta(const Meta& other) { format_ = other.format_; }
|
||||
|
||||
std::variant<std::vector<double>, std::vector<Eigen::VectorXd>> data_;
|
||||
|
||||
std::string format_;
|
||||
};
|
||||
|
||||
Meta& add(const std::string& name, double value);
|
||||
Meta& add(const std::string& name, const Eigen::VectorXd& value);
|
||||
Meta& add(const std::string& name, const Eigen::VectorXf& value);
|
||||
|
||||
void merge_all(const ExecutionStats& other);
|
||||
|
||||
void merge_sums(const ExecutionStats& other);
|
||||
|
||||
void print() const;
|
||||
|
||||
bool save_json(const std::string& path) const;
|
||||
|
||||
private:
|
||||
std::unordered_map<std::string, Meta> stats_;
|
||||
std::vector<std::string> order_;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
222
include/basalt/utils/tracks.h
Normal file
222
include/basalt/utils/tracks.h
Normal file
@@ -0,0 +1,222 @@
|
||||
// Adapted from OpenMVG
|
||||
|
||||
// Copyright (c) 2012, 2013 Pierre MOULON
|
||||
// 2018 Nikolaus DEMMEL
|
||||
|
||||
// This file was originally part of OpenMVG, an Open Multiple View Geometry C++
|
||||
// library.
|
||||
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
// Implementation of [1] an efficient algorithm to compute track from pairwise
|
||||
// correspondences.
|
||||
//
|
||||
// [1] Pierre Moulon and Pascal Monasse,
|
||||
// "Unordered feature tracking made fast and easy" CVMP 2012.
|
||||
//
|
||||
// It tracks the position of features along the series of image from pairwise
|
||||
// correspondences.
|
||||
//
|
||||
// From map<[imageI,ImageJ], [indexed matches array] > it builds tracks.
|
||||
//
|
||||
// Usage :
|
||||
// //---------------------------------------
|
||||
// // Compute tracks from matches
|
||||
// //---------------------------------------
|
||||
// TrackBuilder trackBuilder;
|
||||
// FeatureTracks tracks;
|
||||
// trackBuilder.Build(matches); // Build: Efficient fusion of correspondences
|
||||
// trackBuilder.Filter(); // Filter: Remove tracks that have conflict
|
||||
// trackBuilder.Export(tracks); // Export tree to usable data structure
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <iterator>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <basalt/utils/common_types.h>
|
||||
#include <basalt/utils/union_find.h>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
/// TrackBuild class creates feature tracks from matches
|
||||
struct TrackBuilder {
|
||||
std::map<ImageFeaturePair, TrackId> map_node_to_index;
|
||||
UnionFind uf_tree;
|
||||
|
||||
/// Build tracks for a given series of pairWise matches
|
||||
void Build(const Matches& map_pair_wise_matches) {
|
||||
// 1. We need to know how much single set we will have.
|
||||
// i.e each set is made of a tuple : (imageIndex, featureIndex)
|
||||
std::set<ImageFeaturePair> allFeatures;
|
||||
// For each couple of images list the used features
|
||||
for (const auto& iter : map_pair_wise_matches) {
|
||||
const auto I = iter.first.first;
|
||||
const auto J = iter.first.second;
|
||||
const MatchData& matchData = iter.second;
|
||||
|
||||
// Retrieve all shared features and add them to a set
|
||||
for (const auto& match : matchData.inliers) {
|
||||
allFeatures.emplace(I, match.first);
|
||||
allFeatures.emplace(J, match.second);
|
||||
}
|
||||
}
|
||||
|
||||
// 2. Build the 'flat' representation where a tuple (the node)
|
||||
// is attached to a unique index.
|
||||
TrackId cpt = 0;
|
||||
for (const auto& feat : allFeatures) {
|
||||
map_node_to_index.emplace(feat, cpt);
|
||||
++cpt;
|
||||
}
|
||||
// Clean some memory
|
||||
allFeatures.clear();
|
||||
|
||||
// 3. Add the node and the pairwise correpondences in the UF tree.
|
||||
uf_tree.InitSets(map_node_to_index.size());
|
||||
|
||||
// 4. Union of the matched features corresponding UF tree sets
|
||||
for (const auto& iter : map_pair_wise_matches) {
|
||||
const auto I = iter.first.first;
|
||||
const auto J = iter.first.second;
|
||||
const MatchData& matchData = iter.second;
|
||||
for (const auto& match : matchData.inliers) {
|
||||
const ImageFeaturePair pairI(I, match.first);
|
||||
const ImageFeaturePair pairJ(J, match.second);
|
||||
// Link feature correspondences to the corresponding containing sets.
|
||||
uf_tree.Union(map_node_to_index[pairI], map_node_to_index[pairJ]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Remove bad tracks (too short or track with ids collision)
|
||||
bool Filter(size_t minimumTrackLength = 2) {
|
||||
// Remove bad tracks:
|
||||
// - track that are too short,
|
||||
// - track with id conflicts:
|
||||
// i.e. tracks that have many times the same image index
|
||||
|
||||
// From the UF tree, create tracks of the image indexes.
|
||||
// If an image index appears twice the track must disappear
|
||||
// If a track is too short it has to be removed.
|
||||
std::map<TrackId, std::set<TimeCamId>> tracks;
|
||||
|
||||
std::set<TrackId> problematic_track_id;
|
||||
// Build tracks from the UF tree, track problematic ids.
|
||||
for (const auto& iter : map_node_to_index) {
|
||||
const TrackId track_id = uf_tree.Find(iter.second);
|
||||
if (problematic_track_id.count(track_id) != 0) {
|
||||
continue; // Track already marked
|
||||
}
|
||||
|
||||
const ImageFeaturePair& feat = iter.first;
|
||||
|
||||
if (tracks[track_id].count(feat.first)) {
|
||||
problematic_track_id.insert(track_id);
|
||||
} else {
|
||||
tracks[track_id].insert(feat.first);
|
||||
}
|
||||
}
|
||||
|
||||
// - track that are too short,
|
||||
for (const auto& val : tracks) {
|
||||
if (val.second.size() < minimumTrackLength) {
|
||||
problematic_track_id.insert(val.first);
|
||||
}
|
||||
}
|
||||
|
||||
for (uint32_t& root_index : uf_tree.m_cc_parent) {
|
||||
if (problematic_track_id.count(root_index) > 0) {
|
||||
// reset selected root
|
||||
uf_tree.m_cc_size[root_index] = 1;
|
||||
root_index = UnionFind::InvalidIndex();
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/// Return the number of connected set in the UnionFind structure (tree
|
||||
/// forest)
|
||||
size_t TrackCount() const {
|
||||
std::set<TrackId> parent_id(uf_tree.m_cc_parent.begin(),
|
||||
uf_tree.m_cc_parent.end());
|
||||
// Erase the "special marker" that depicted rejected tracks
|
||||
parent_id.erase(UnionFind::InvalidIndex());
|
||||
return parent_id.size();
|
||||
}
|
||||
|
||||
/// Export tracks as a map (each entry is a map of imageId and
|
||||
/// featureIndex):
|
||||
/// {TrackIndex => {imageIndex => featureIndex}}
|
||||
void Export(FeatureTracks& tracks) {
|
||||
tracks.clear();
|
||||
for (const auto& iter : map_node_to_index) {
|
||||
const TrackId track_id = uf_tree.Find(iter.second);
|
||||
const ImageFeaturePair& feat = iter.first;
|
||||
// ensure never add rejected elements (track marked as invalid)
|
||||
if (track_id != UnionFind::InvalidIndex()) {
|
||||
tracks[track_id].emplace(feat);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/// Find common tracks between images.
|
||||
bool GetTracksInImages(const std::set<TimeCamId>& image_ids,
|
||||
const FeatureTracks& all_tracks,
|
||||
std::vector<TrackId>& shared_track_ids) {
|
||||
shared_track_ids.clear();
|
||||
|
||||
// Go along the tracks
|
||||
for (const auto& kv_track : all_tracks) {
|
||||
// Look if the track contains the provided view index & save the point ids
|
||||
size_t observed_image_count = 0;
|
||||
for (const auto& imageId : image_ids) {
|
||||
if (kv_track.second.count(imageId) > 0) {
|
||||
++observed_image_count;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (observed_image_count == image_ids.size()) {
|
||||
shared_track_ids.push_back(kv_track.first);
|
||||
}
|
||||
}
|
||||
return !shared_track_ids.empty();
|
||||
}
|
||||
|
||||
/// Find all tracks in an image.
|
||||
bool GetTracksInImage(const TimeCamId& image_id,
|
||||
const FeatureTracks& all_tracks,
|
||||
std::vector<TrackId>& track_ids) {
|
||||
std::set<TimeCamId> image_set;
|
||||
image_set.insert(image_id);
|
||||
return GetTracksInImages(image_set, all_tracks, track_ids);
|
||||
}
|
||||
|
||||
/// Find shared tracks between map and image
|
||||
bool GetSharedTracks(const TimeCamId& image_id, const FeatureTracks& all_tracks,
|
||||
const Landmarks& landmarks,
|
||||
std::vector<TrackId>& track_ids) {
|
||||
track_ids.clear();
|
||||
for (const auto& kv : landmarks) {
|
||||
const TrackId trackId = kv.first;
|
||||
if (all_tracks.at(trackId).count(image_id) > 0) {
|
||||
track_ids.push_back(trackId);
|
||||
}
|
||||
}
|
||||
return !track_ids.empty();
|
||||
}
|
||||
|
||||
} // namespace basalt
|
||||
94
include/basalt/utils/union_find.h
Normal file
94
include/basalt/utils/union_find.h
Normal file
@@ -0,0 +1,94 @@
|
||||
// Adapted from OpenMVG
|
||||
|
||||
// Copyright (c) 2016 Pierre MOULON
|
||||
// 2018 Nikolaus DEMMEL
|
||||
|
||||
// This file was originally part of OpenMVG, an Open Multiple View Geometry C++
|
||||
// library.
|
||||
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <vector>
|
||||
|
||||
// Union-Find/Disjoint-Set data structure
|
||||
//--
|
||||
// A disjoint-set data structure also called a union–find data structure
|
||||
// or merge–find set, is a data structure that keeps track of a set of elements
|
||||
// partitioned into a number of disjoint (non-overlapping) subsets.
|
||||
// It supports two operations:
|
||||
// - Find: Determine which subset a particular element is in.
|
||||
// - It returns an item from this set that serves as its "representative";
|
||||
// - Union: Join two subsets into a single subset.
|
||||
// Sometime a Connected method is implemented:
|
||||
// - Connected:
|
||||
// - By comparing the result of two Find operations, one can determine whether
|
||||
// two elements are in the same subset.
|
||||
//--
|
||||
struct UnionFind {
|
||||
using ValueType = uint32_t;
|
||||
|
||||
// Special Value for invalid parent index
|
||||
static ValueType InvalidIndex() {
|
||||
return std::numeric_limits<ValueType>::max();
|
||||
}
|
||||
|
||||
// Represent the DS/UF forest thanks to two array:
|
||||
// A parent 'pointer tree' where each node holds a reference to its parent
|
||||
// node
|
||||
std::vector<ValueType> m_cc_parent;
|
||||
// A rank array used for union by rank
|
||||
std::vector<ValueType> m_cc_rank;
|
||||
// A 'size array' to know the size of each connected component
|
||||
std::vector<ValueType> m_cc_size;
|
||||
|
||||
// Init the UF structure with num_cc nodes
|
||||
void InitSets(const ValueType num_cc) {
|
||||
// all set size are 1 (independent nodes)
|
||||
m_cc_size.resize(num_cc, 1);
|
||||
// Parents id have their own CC id {0,n}
|
||||
m_cc_parent.resize(num_cc);
|
||||
std::iota(m_cc_parent.begin(), m_cc_parent.end(), 0);
|
||||
// Rank array (0)
|
||||
m_cc_rank.resize(num_cc, 0);
|
||||
}
|
||||
|
||||
// Return the number of nodes that have been initialized in the UF tree
|
||||
std::size_t GetNumNodes() const { return m_cc_size.size(); }
|
||||
|
||||
// Return the representative set id of I nth component
|
||||
ValueType Find(ValueType i) {
|
||||
// Recursively set all branch as children of root (Path compression)
|
||||
if (m_cc_parent[i] != i && m_cc_parent[i] != InvalidIndex())
|
||||
m_cc_parent[i] = Find(m_cc_parent[i]);
|
||||
return m_cc_parent[i];
|
||||
}
|
||||
|
||||
// Replace sets containing I and J with their union
|
||||
void Union(ValueType i, ValueType j) {
|
||||
i = Find(i);
|
||||
j = Find(j);
|
||||
if (i == j) { // Already in the same set. Nothing to do
|
||||
return;
|
||||
}
|
||||
|
||||
// x and y are not already in same set. Merge them.
|
||||
// Perform an union by rank:
|
||||
// - always attach the smaller tree to the root of the larger tree
|
||||
if (m_cc_rank[i] < m_cc_rank[j]) {
|
||||
m_cc_parent[i] = j;
|
||||
m_cc_size[j] += m_cc_size[i];
|
||||
|
||||
} else {
|
||||
m_cc_parent[j] = i;
|
||||
m_cc_size[i] += m_cc_size[j];
|
||||
if (m_cc_rank[i] == m_cc_rank[j]) ++m_cc_rank[i];
|
||||
}
|
||||
}
|
||||
};
|
||||
111
include/basalt/utils/vio_config.h
Normal file
111
include/basalt/utils/vio_config.h
Normal file
@@ -0,0 +1,111 @@
|
||||
/**
|
||||
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 <string>
|
||||
|
||||
namespace basalt {
|
||||
|
||||
enum class LinearizationType { ABS_QR, ABS_SC, REL_SC };
|
||||
|
||||
struct VioConfig {
|
||||
VioConfig();
|
||||
void load(const std::string& filename);
|
||||
void save(const std::string& filename);
|
||||
|
||||
std::string optical_flow_type;
|
||||
int optical_flow_detection_grid_size;
|
||||
float optical_flow_max_recovered_dist2;
|
||||
int optical_flow_pattern;
|
||||
int optical_flow_max_iterations;
|
||||
int optical_flow_levels;
|
||||
float optical_flow_epipolar_error;
|
||||
int optical_flow_skip_frames;
|
||||
|
||||
LinearizationType vio_linearization_type;
|
||||
bool vio_sqrt_marg;
|
||||
|
||||
int vio_max_states;
|
||||
int vio_max_kfs;
|
||||
int vio_min_frames_after_kf;
|
||||
float vio_new_kf_keypoints_thresh;
|
||||
bool vio_debug;
|
||||
bool vio_extended_logging;
|
||||
|
||||
// double vio_outlier_threshold;
|
||||
// int vio_filter_iteration;
|
||||
int vio_max_iterations;
|
||||
|
||||
double vio_obs_std_dev;
|
||||
double vio_obs_huber_thresh;
|
||||
double vio_min_triangulation_dist;
|
||||
|
||||
bool vio_enforce_realtime;
|
||||
|
||||
bool vio_use_lm;
|
||||
double vio_lm_lambda_initial;
|
||||
double vio_lm_lambda_min;
|
||||
double vio_lm_lambda_max;
|
||||
|
||||
bool vio_scale_jacobian;
|
||||
|
||||
double vio_init_pose_weight;
|
||||
double vio_init_ba_weight;
|
||||
double vio_init_bg_weight;
|
||||
|
||||
bool vio_marg_lost_landmarks;
|
||||
double vio_kf_marg_feature_ratio;
|
||||
|
||||
double mapper_obs_std_dev;
|
||||
double mapper_obs_huber_thresh;
|
||||
int mapper_detection_num_points;
|
||||
double mapper_num_frames_to_match;
|
||||
double mapper_frames_to_match_threshold;
|
||||
double mapper_min_matches;
|
||||
double mapper_ransac_threshold;
|
||||
double mapper_min_track_length;
|
||||
double mapper_max_hamming_distance;
|
||||
double mapper_second_best_test_ratio;
|
||||
int mapper_bow_num_bits;
|
||||
double mapper_min_triangulation_dist;
|
||||
bool mapper_no_factor_weights;
|
||||
bool mapper_use_factors;
|
||||
|
||||
bool mapper_use_lm;
|
||||
double mapper_lm_lambda_min;
|
||||
double mapper_lm_lambda_max;
|
||||
};
|
||||
|
||||
} // namespace basalt
|
||||
106
include/basalt/utils/vis_utils.h
Normal file
106
include/basalt/utils/vis_utils.h
Normal file
@@ -0,0 +1,106 @@
|
||||
/**
|
||||
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 <pangolin/gl/gldraw.h>
|
||||
|
||||
#include <basalt/utils/sophus_utils.hpp>
|
||||
|
||||
const uint8_t cam_color[3]{250, 0, 26};
|
||||
const uint8_t state_color[3]{250, 0, 26};
|
||||
const uint8_t pose_color[3]{0, 50, 255};
|
||||
const uint8_t gt_color[3]{0, 171, 47};
|
||||
|
||||
inline void render_camera(const Eigen::Matrix4d& T_w_c, float lineWidth,
|
||||
const uint8_t* color, float sizeFactor) {
|
||||
const float sz = sizeFactor;
|
||||
const float width = 640, height = 480, fx = 500, fy = 500, cx = 320, cy = 240;
|
||||
|
||||
const Eigen::aligned_vector<Eigen::Vector3f> lines = {
|
||||
{0, 0, 0},
|
||||
{sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz},
|
||||
{0, 0, 0},
|
||||
{sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
|
||||
{0, 0, 0},
|
||||
{sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
|
||||
{0, 0, 0},
|
||||
{sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz},
|
||||
{sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz},
|
||||
{sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
|
||||
{sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
|
||||
{sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
|
||||
{sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
|
||||
{sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz},
|
||||
{sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz},
|
||||
{sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz}};
|
||||
|
||||
glPushMatrix();
|
||||
glMultMatrixd(T_w_c.data());
|
||||
glColor3ubv(color);
|
||||
glLineWidth(lineWidth);
|
||||
pangolin::glDrawLines(lines);
|
||||
glPopMatrix();
|
||||
}
|
||||
|
||||
inline void getcolor(float p, float np, float& r, float& g, float& b) {
|
||||
float inc = 4.0 / np;
|
||||
float x = p * inc;
|
||||
r = 0.0f;
|
||||
g = 0.0f;
|
||||
b = 0.0f;
|
||||
|
||||
if ((0 <= x && x <= 1) || (5 <= x && x <= 6))
|
||||
r = 1.0f;
|
||||
else if (4 <= x && x <= 5)
|
||||
r = x - 4;
|
||||
else if (1 <= x && x <= 2)
|
||||
r = 1.0f - (x - 1);
|
||||
|
||||
if (1 <= x && x <= 3)
|
||||
g = 1.0f;
|
||||
else if (0 <= x && x <= 1)
|
||||
g = x - 0;
|
||||
else if (3 <= x && x <= 4)
|
||||
g = 1.0f - (x - 3);
|
||||
|
||||
if (3 <= x && x <= 5)
|
||||
b = 1.0f;
|
||||
else if (2 <= x && x <= 3)
|
||||
b = x - 2;
|
||||
else if (5 <= x && x <= 6)
|
||||
b = 1.0f - (x - 5);
|
||||
}
|
||||
167
include/basalt/vi_estimator/ba_base.h
Normal file
167
include/basalt/vi_estimator/ba_base.h
Normal 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
156
include/basalt/vi_estimator/landmark_database.h
Normal 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
68
include/basalt/vi_estimator/marg_helper.h
Normal 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
209
include/basalt/vi_estimator/nfr_mapper.h
Normal 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
452
include/basalt/vi_estimator/sc_ba_base.h
Normal 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
124
include/basalt/vi_estimator/sqrt_ba_base.h
Normal 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
261
include/basalt/vi_estimator/sqrt_keypoint_vio.h
Normal 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
251
include/basalt/vi_estimator/sqrt_keypoint_vo.h
Normal 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
136
include/basalt/vi_estimator/vio_estimator.h
Normal 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
|
||||
Reference in New Issue
Block a user