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
|
||||
Reference in New Issue
Block a user