i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

OpenVINS Update (#1107)

Browse Source 
* fix openvins build

* Add OpenVINS params to UI

* update OdometryOpenVINS implementation

* the pixel noise of most cameras should be less than 3 after factory calibration

* propagation and update are only performed after camera measurement feeding

* fix openvins feature reprojection
This commit is contained in:
Borong Yuan
2023-10-02 22:53:29 +08:00
committed by GitHub
parent 78c029d833
commit 67c6a15b93
5 changed files with 1441 additions and 391 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
corelib/include/rtabmap/core/Parameters.h
View File

@@ -576,6 +576,48 @@ class RTABMAP_CORE_EXPORT Parameters
// Odometry VINS // Odometry VINS
RTABMAP_PARAM_STR(OdomVINS, ConfigPath, "", "Path of VINS config file."); RTABMAP_PARAM_STR(OdomVINS, ConfigPath, "", "Path of VINS config file.");
// Odometry OpenVINS
RTABMAP_PARAM(OdomOpenVINS, UseStereo, bool, true, "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
RTABMAP_PARAM(OdomOpenVINS, UseKLT, bool, true, "If true we will use KLT, otherwise use a ORB descriptor + robust matching");
RTABMAP_PARAM(OdomOpenVINS, NumPts, int, 200, "Number of points (per camera) we will extract and try to track");
RTABMAP_PARAM(OdomOpenVINS, FastThreshold, int, 30, "Threshold for fast extraction (warning: lower threshs can be expensive)");
RTABMAP_PARAM(OdomOpenVINS, GridX, int, 5, "Extraction sub-grid count for horizontal direction (uniform tracking)");
RTABMAP_PARAM(OdomOpenVINS, GridY, int, 5, "Extraction sub-grid count for vertical direction (uniform tracking)");
RTABMAP_PARAM(OdomOpenVINS, MinPxDist, int, 15, "Eistance between features (features near each other provide less information)");
RTABMAP_PARAM(OdomOpenVINS, KNNRatio, double, 0.7, "Descriptor knn threshold for the top two descriptor matches");
RTABMAP_PARAM(OdomOpenVINS, UseFEJ, bool, true, "If first-estimate Jacobians should be used (enable for good consistency)");
RTABMAP_PARAM(OdomOpenVINS, Integration, int, 1, "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)");
RTABMAP_PARAM(OdomOpenVINS, MaxClones, int, 11, "Max clone size of sliding window");
RTABMAP_PARAM(OdomOpenVINS, MaxSLAM, int, 50, "Max number of estimated SLAM features");
RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate, int, 25, "Max number of SLAM features we allow to be included in a single EKF update.");
RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate, int, 50, "Max number of MSCKF features we will use at a given image timestep.");
RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF, int, 0, "What representation our features are in (msckf features)");
RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM, int, 4, "What representation our features are in (slam features)");
RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay, double, 2.0, "Delay, in seconds, that we should wait from init before we start estimating SLAM features");
RTABMAP_PARAM(OdomOpenVINS, GravityMag, double, 9.81, "Gravity magnitude in the global frame (i.e. should be 9.81 typically)");
RTABMAP_PARAM(OdomOpenVINS, InitWindowTime, double, 2.0, "Amount of time we will initialize over (seconds)");
RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh, double, 1.0, "Variance threshold on our acceleration to be classified as moving");
RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity, double, 10.0, "Max disparity to consider the platform stationary (dependent on resolution)");
RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures, int, 50, "How many features to track during initialization (saves on computation)");
RTABMAP_PARAM(OdomOpenVINS, TryZUPT, bool, true, "If we should try to use zero velocity update");
RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier, double, 0.0, "Chi2 multiplier for zero velocity");
RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy, double, 0.1, "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier, double, 10.0, "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)");
RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity, double, 0.5, "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning, bool, false, "If we should only use the zupt at the very beginning static initialization phase");
RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01, "[m/s^2/sqrt(Hz)] (accel \"white noise\")");
RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk, double, 0.001, "[m/s^3/sqrt(Hz)] (accel bias diffusion)");
RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity, double, 0.001, "[rad/s/sqrt(Hz)] (gyro \"white noise\")");
RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk, double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)");
RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx, double, 2.0, "Pixel noise for MSCKF features");
RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features");
RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 2.0, "Pixel noise for SLAM features");
RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features");
// Odometry Open3D // Odometry Open3D
RTABMAP_PARAM(OdomOpen3D, MaxDepth, float, 3.0, "Maximum depth."); RTABMAP_PARAM(OdomOpen3D, MaxDepth, float, 3.0, "Maximum depth.");
RTABMAP_PARAM(OdomOpen3D, Method, int, 0, "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid."); RTABMAP_PARAM(OdomOpen3D, Method, int, 0, "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");

11
corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace ov_msckf { namespace ov_msckf {
class VioManager; class VioManager;
struct VioManagerOptions;
} }
namespace rtabmap { namespace rtabmap {
@@ -40,7 +41,6 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry
{ {
public: public:
OdometryOpenVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); OdometryOpenVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryOpenVINS();
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeOpenVINS;} virtual Odometry::Type getType() {return Odometry::kTypeOpenVINS;}
@@ -52,12 +52,11 @@ private:
private: private:
#ifdef RTABMAP_OPENVINS #ifdef RTABMAP_OPENVINS
ov_msckf::VioManager * vioManager_; std::unique_ptr<ov_msckf::VioManager> vioManager_;
std::unique_ptr<ov_msckf::VioManagerOptions> params_;
bool initGravity_; bool initGravity_;
Transform previousPose_; Transform previousPoseInv_;
Transform previousLocalTransform_; Transform imuLocalTransformInv_;
Transform imuLocalTransform_;
std::map<double, IMU> imuBuffer_;
#endif #endif
}; };

668
corelib/src/odometry/OdometryOpenVINS.cpp
View File

@@ -30,20 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UThread.h"
#include "rtabmap/utilite/UDirectory.h"
#include <opencv2/imgproc/types_c.h> #include <opencv2/imgproc/types_c.h>
#ifdef RTABMAP_OPENVINS #ifdef RTABMAP_OPENVINS
#include "core/VioManager.h" #include "core/VioManager.h"
#include "core/VioManagerOptions.h"
#include "core/RosVisualizer.h"
#include "utils/dataset_reader.h"
#include "utils/parse_ros.h"
#include "utils/sensor_data.h"
#include "state/State.h" #include "state/State.h"
#include "types/Type.h" #include "state/StateHelper.h"
#endif #endif
namespace rtabmap { namespace rtabmap {
@@ -52,17 +44,67 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
Odometry(parameters) Odometry(parameters)
#ifdef RTABMAP_OPENVINS #ifdef RTABMAP_OPENVINS
, ,
vioManager_(0),
initGravity_(false), initGravity_(false),
previousPose_(Transform::getIdentity()) previousPoseInv_(Transform::getIdentity())
#endif #endif
{ {
}
OdometryOpenVINS::~OdometryOpenVINS()
{
#ifdef RTABMAP_OPENVINS #ifdef RTABMAP_OPENVINS
delete vioManager_; ov_core::Printer::setPrintLevel("WARNING");
int enum_index;
params_ = std::make_unique<ov_msckf::VioManagerOptions>();
Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo);
Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt);
Parameters::parse(parameters, Parameters::kOdomOpenVINSNumPts(), params_->num_pts);
Parameters::parse(parameters, Parameters::kOdomOpenVINSFastThreshold(), params_->fast_threshold);
Parameters::parse(parameters, Parameters::kOdomOpenVINSGridX(), params_->grid_x);
Parameters::parse(parameters, Parameters::kOdomOpenVINSGridY(), params_->grid_y);
Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist);
Parameters::parse(parameters, Parameters::kOdomOpenVINSKNNRatio(), params_->knn_ratio);
Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej);
Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index);
params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index);
Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxClones(), params_->state_options.max_clone_size);
Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAM(), params_->state_options.max_slam_features);
Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAMInUpdate(), params_->state_options.max_slam_in_update);
Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxMSCKFInUpdate(), params_->state_options.max_msckf_in_update);
Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepMSCKF(), enum_index);
params_->state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::Representation(enum_index);
Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepSLAM(), enum_index);
params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index);
Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay);
Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag);
Parameters::parse(parameters, Parameters::kOdomOpenVINSInitWindowTime(), params_->init_options.init_window_time);
Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh);
Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity);
Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxFeatures(), params_->init_options.init_max_features);
Parameters::parse(parameters, Parameters::kOdomOpenVINSTryZUPT(), params_->try_zupt);
Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTChi2Multiplier(), params_->zupt_options.chi2_multipler);
Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxVelodicy(), params_->zupt_max_velocity);
Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTNoiseMultiplier(), params_->zupt_noise_multiplier);
Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxDisparity(), params_->zupt_max_disparity);
Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTOnlyAtBeginning(), params_->zupt_only_at_beginning);
Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerNoiseDensity(), params_->imu_noises.sigma_a);
Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerRandomWalk(), params_->imu_noises.sigma_ab);
Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeNoiseDensity(), params_->imu_noises.sigma_w);
Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeRandomWalk(), params_->imu_noises.sigma_wb);
Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFSigmaPx(), params_->msckf_options.sigma_pix);
Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier(), params_->msckf_options.chi2_multipler);
Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMSigmaPx(), params_->slam_options.sigma_pix);
Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMChi2Multiplier(), params_->slam_options.chi2_multipler);
params_->vec_dw << 1, 0, 0, 1, 0, 1;
params_->vec_da << 1, 0, 0, 1, 0, 1;
params_->vec_tg << 0, 0, 0, 0, 0, 0, 0, 0, 0;
params_->q_ACCtoIMU << 0, 0, 0, 1;
params_->q_GYROtoIMU << 0, 0, 0, 1;
params_->use_aruco = false;
params_->num_opencv_threads = -1;
params_->histogram_method = ov_core::TrackBase::HistogramMethod::NONE;
params_->init_options.sigma_a = params_->imu_noises.sigma_a;
params_->init_options.sigma_ab = params_->imu_noises.sigma_ab;
params_->init_options.sigma_w = params_->imu_noises.sigma_w;
params_->init_options.sigma_wb = params_->imu_noises.sigma_wb;
params_->init_options.sigma_pix = params_->slam_options.sigma_pix;
params_->init_options.gravity_mag = params_->gravity_mag;
#endif #endif
} }
@@ -72,11 +114,9 @@ void OdometryOpenVINS::reset(const Transform & initialPose)
#ifdef RTABMAP_OPENVINS #ifdef RTABMAP_OPENVINS
if(!initGravity_) if(!initGravity_)
{ {
delete vioManager_; vioManager_.reset();
vioManager_ = 0; previousPoseInv_.setIdentity();
previousPose_.setIdentity(); imuLocalTransformInv_.setNull();
previousLocalTransform_.setNull();
imuBuffer_.clear();
} }
initGravity_ = false; initGravity_ = false;
#endif #endif
@@ -90,394 +130,256 @@ Transform OdometryOpenVINS::computeTransform(
{ {
Transform t; Transform t;
#ifdef RTABMAP_OPENVINS #ifdef RTABMAP_OPENVINS
UTimer timer;
// Buffer imus; if(!vioManager_)
if(!data.imu().empty())
{ {
imuBuffer_.insert(std::make_pair(data.stamp(), data.imu())); if(!data.imu().empty())
} imuLocalTransformInv_ = data.imu().localTransform().inverse();
// OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull())
if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1)
{
if(imuBuffer_.empty())
{ {
UWARN("Waiting IMU for initialization..."); Transform T_imu_left;
return t; Eigen::VectorXd left_calib(8), right_calib(8);
} if(!data.rightRaw().empty())
if(vioManager_ == 0)
{
UINFO("OpenVINS Initialization");
// intialize
ov_msckf::VioManagerOptions params;
// ESTIMATOR ======================================================================
// Main EKF parameters
//params.state_options.do_fej = true;
//params.state_options.imu_avg =false;
//params.state_options.use_rk4_integration;
//params.state_options.do_calib_camera_pose = false;
//params.state_options.do_calib_camera_intrinsics = false;
//params.state_options.do_calib_camera_timeoffset = false;
//params.state_options.max_clone_size = 11;
//params.state_options.max_slam_features = 25;
//params.state_options.max_slam_in_update = INT_MAX;
//params.state_options.max_msckf_in_update = INT_MAX;
//params.state_options.max_aruco_features = 1024;
params.state_options.num_cameras = 2;
//params.dt_slam_delay = 2;
// Set what representation we should be using
//params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
//params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
//params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
if( params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN ||
params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN ||
params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN)
{ {
printf(RED "VioManager(): invalid feature representation specified:\n" RESET); params_->state_options.num_cameras = params_->init_options.num_cameras = 2;
printf(RED "\t- GLOBAL_3D\n" RESET); T_imu_left = imuLocalTransformInv_ * data.stereoCameraModels()[0].localTransform();
printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
printf(RED "\t- ANCHORED_3D\n" RESET);
printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
std::exit(EXIT_FAILURE);
}
// Filter initialization bool is_fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
//params.init_window_time = 1; if(is_fisheye)
//params.init_imu_thresh = 1; {
params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
params_->camera_intrinsics.emplace(1, std::make_shared<ov_core::CamEqui>(
data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight()));
}
else
{
params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
params_->camera_intrinsics.emplace(1, std::make_shared<ov_core::CamRadtan>(
data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight()));
}
// Zero velocity update if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
//params.try_zupt = false; {
//params.zupt_options.chi2_multipler = 5; left_calib << data.stereoCameraModels()[0].left().fx(),
//params.zupt_max_velocity = 1; data.stereoCameraModels()[0].left().fy(),
//params.zupt_noise_multiplier = 1; data.stereoCameraModels()[0].left().cx(),
data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
// NOISE ====================================================================== right_calib << data.stereoCameraModels()[0].right().fx(),
data.stereoCameraModels()[0].right().fy(),
// Our noise values for inertial sensor data.stereoCameraModels()[0].right().cx(),
//params.imu_noises.sigma_w = 1.6968e-04; data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
//params.imu_noises.sigma_a = 2.0000e-3; }
//params.imu_noises.sigma_wb = 1.9393e-05; else
//params.imu_noises.sigma_ab = 3.0000e-03; {
UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
// Read in update parameters UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
//params.msckf_options.sigma_pix = 1; UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
//params.msckf_options.chi2_multipler = 5; left_calib << data.stereoCameraModels()[0].left().K_raw().at<double>(0,0),
//params.slam_options.sigma_pix = 1; data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
//params.slam_options.chi2_multipler = 5; data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
//params.aruco_options.sigma_pix = 1; data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
//params.aruco_options.chi2_multipler = 5; data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,is_fisheye?4:2),
// STATE ====================================================================== data.stereoCameraModels()[0].left().D_raw().at<double>(0,is_fisheye?5:3);
right_calib << data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
// Timeoffset from camera to IMU data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
//params.calib_camimu_dt = 0.0; data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
// Global gravity data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
//params.gravity[2] = 9.81; data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,is_fisheye?4:2),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,is_fisheye?5:3);
// TRACKERS ====================================================================== }
// Tracking flags
params.use_stereo = true;
//params.use_klt = true;
params.use_aruco = false;
//params.downsize_aruco = true;
//params.downsample_cameras = false;
//params.use_multi_threading = true;
// General parameters
//params.num_pts = 200;
//params.fast_threshold = 10;
//params.grid_x = 10;
//params.grid_y = 5;
//params.min_px_dist = 8;
//params.knn_ratio = 0.7;
// Feature initializer parameters
//nh.param<bool>("fi_triangulate_1d", params.featinit_options.triangulate_1d, params.featinit_options.triangulate_1d);
//nh.param<bool>("fi_refine_features", params.featinit_options.refine_features, params.featinit_options.refine_features);
//nh.param<int>("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs);
//nh.param<double>("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda);
//nh.param<double>("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda);
//nh.param<double>("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx);
///nh.param<double>("fi_min_dcost", params.featinit_options.min_dcost, params.featinit_options.min_dcost);
//nh.param<double>("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult);
//nh.param<double>("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist);
//params.featinit_options.max_dist = 75;
//params.featinit_options.max_baseline = 500;
//params.featinit_options.max_cond_number = 5000;
// CAMERA ======================================================================
bool fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
params.camera_fisheye.insert(std::make_pair(0, fisheye));
params.camera_fisheye.insert(std::make_pair(1, fisheye));
Eigen::VectorXd camLeft(8), camRight(8);
if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
{
camLeft << data.stereoCameraModels()[0].left().fx(),
data.stereoCameraModels()[0].left().fy(),
data.stereoCameraModels()[0].left().cx(),
data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
camRight << data.stereoCameraModels()[0].right().fx(),
data.stereoCameraModels()[0].right().fy(),
data.stereoCameraModels()[0].right().cx(),
data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
} }
else else
{ {
UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols); params_->state_options.num_cameras = params_->init_options.num_cameras = 1;
UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4); T_imu_left = imuLocalTransformInv_ * data.cameraModels()[0].localTransform();
UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
//https://github.com/ethz-asl/kalibr/wiki/supported-models bool is_fisheye = data.cameraModels()[0].isFisheye() && !this->imagesAlreadyRectified();
/// radial-tangential (radtan) if(is_fisheye)
// (distortion_coeffs: [k1 k2 r1 r2]) {
/// equidistant (equi) params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
// (distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4) data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
}
else
{
params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
}
camLeft << if(this->imagesAlreadyRectified() || data.cameraModels()[0].D_raw().empty())
data.stereoCameraModels()[0].left().K_raw().at<double>(0,0), {
data.stereoCameraModels()[0].left().K_raw().at<double>(1,1), left_calib << data.cameraModels()[0].fx(),
data.stereoCameraModels()[0].left().K_raw().at<double>(0,2), data.cameraModels()[0].fy(),
data.stereoCameraModels()[0].left().K_raw().at<double>(1,2), data.cameraModels()[0].cx(),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,0), data.cameraModels()[0].cy(), 0, 0, 0, 0;
data.stereoCameraModels()[0].left().D_raw().at<double>(0,1), }
data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?4:2), else
data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?5:3); {
camRight << UASSERT(data.cameraModels()[0].D_raw().cols >= 4);
data.stereoCameraModels()[0].right().K_raw().at<double>(0,0), left_calib << data.cameraModels()[0].K_raw().at<double>(0,0),
data.stereoCameraModels()[0].right().K_raw().at<double>(1,1), data.cameraModels()[0].K_raw().at<double>(1,1),
data.stereoCameraModels()[0].right().K_raw().at<double>(0,2), data.cameraModels()[0].K_raw().at<double>(0,2),
data.stereoCameraModels()[0].right().K_raw().at<double>(1,2), data.cameraModels()[0].K_raw().at<double>(1,2),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,0), data.cameraModels()[0].D_raw().at<double>(0,0),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,1), data.cameraModels()[0].D_raw().at<double>(0,1),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?4:2), data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?4:2),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?5:3); data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?5:3);
}
} }
params.camera_intrinsics.insert(std::make_pair(0, camLeft));
params.camera_intrinsics.insert(std::make_pair(1, camRight));
const IMU & imu = imuBuffer_.begin()->second; Eigen::Matrix4d T_LtoI = T_imu_left.toEigen4d();
imuLocalTransform_ = imu.localTransform(); Eigen::Matrix<double,7,1> left_eigen;
Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModels()[0].localTransform(); left_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_LtoI.block(0,0,3,3).transpose());
Transform cam0cam1; left_eigen.block(4,0,3,1) = -T_LtoI.block(0,0,3,3).transpose()*T_LtoI.block(0,3,3,1);
if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull()) params_->camera_intrinsics.at(0)->set_value(left_calib);
params_->camera_extrinsics.emplace(0, left_eigen);
if(!data.rightRaw().empty())
{ {
cam0cam1 = Transform( Transform T_left_right;
if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
{
T_left_right = Transform(
1, 0, 0, data.stereoCameraModels()[0].baseline(), 1, 0, 0, data.stereoCameraModels()[0].baseline(),
0, 1, 0, 0, 0, 1, 0, 0,
0, 0, 1, 0); 0, 0, 1, 0);
}
else
{
cam0cam1 = data.stereoCameraModels()[0].stereoTransform().inverse();
}
UASSERT(!cam0cam1.isNull());
Transform imuCam1 = imuCam0 * cam0cam1;
Eigen::Matrix4d cam0_eigen = imuCam0.toEigen4d();
Eigen::Matrix4d cam1_eigen = imuCam1.toEigen4d();
Eigen::Matrix<double,7,1> cam_eigen0;
cam_eigen0.block(0,0,4,1) = rot_2_quat(cam0_eigen.block(0,0,3,3).transpose());
cam_eigen0.block(4,0,3,1) = -cam0_eigen.block(0,0,3,3).transpose()*cam0_eigen.block(0,3,3,1);
Eigen::Matrix<double,7,1> cam_eigen1;
cam_eigen1.block(0,0,4,1) = rot_2_quat(cam1_eigen.block(0,0,3,3).transpose());
cam_eigen1.block(4,0,3,1) = -cam1_eigen.block(0,0,3,3).transpose()*cam1_eigen.block(0,3,3,1);
params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0));
params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1));
params.camera_wh.insert({0, std::make_pair(data.stereoCameraModels()[0].left().imageWidth(),data.stereoCameraModels()[0].left().imageHeight())});
params.camera_wh.insert({1, std::make_pair(data.stereoCameraModels()[0].right().imageWidth(),data.stereoCameraModels()[0].right().imageHeight())});
vioManager_ = new ov_msckf::VioManager(params);
}
cv::Mat left;
cv::Mat right;
if(data.imageRaw().type() == CV_8UC3)
{
cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY);
}
else if(data.imageRaw().type() == CV_8UC1)
{
left = data.imageRaw().clone();
}
else
{
UFATAL("Not supported color type!");
}
if(data.rightRaw().type() == CV_8UC3)
{
cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
}
else if(data.rightRaw().type() == CV_8UC1)
{
right = data.rightRaw().clone();
}
else
{
UFATAL("Not supported color type!");
}
// Create the measurement
ov_core::CameraData message;
message.timestamp = data.stamp();
message.sensor_ids.push_back(0);
message.sensor_ids.push_back(1);
message.images.push_back(left);
message.images.push_back(right);
message.masks.push_back(cv::Mat::zeros(left.size(), CV_8UC1));
message.masks.push_back(cv::Mat::zeros(right.size(), CV_8UC1));
// send it to our VIO system
vioManager_->feed_measurement_camera(message);
UDEBUG("Image update stamp=%f", data.stamp());
double lastIMUstamp = 0.0;
while(!imuBuffer_.empty())
{
std::map<double, IMU>::iterator iter = imuBuffer_.begin();
// Process IMU data until stamp is over image stamp
ov_core::ImuData message;
message.timestamp = iter->first;
message.wm << iter->second.angularVelocity().val[0], iter->second.angularVelocity().val[1], iter->second.angularVelocity().val[2];
message.am << iter->second.linearAcceleration().val[0], iter->second.linearAcceleration().val[1], iter->second.linearAcceleration().val[2];
UDEBUG("IMU update stamp=%f", message.timestamp);
// send it to our VIO system
vioManager_->feed_measurement_imu(message);
lastIMUstamp = iter->first;
imuBuffer_.erase(iter);
if(lastIMUstamp > data.stamp())
{
break;
}
}
if(vioManager_->initialized())
{
// Get the current state
std::shared_ptr<ov_msckf::State> state = vioManager_->get_state();
if(state->_timestamp != data.stamp())
{
UWARN("OpenVINS: Stamp of the current state %f is not the same "
"than last image processed %f (last IMU stamp=%f). There could be "
"a synchronization issue between camera and IMU. ",
state->_timestamp,
data.stamp(),
lastIMUstamp);
}
Transform p(
(float)state->_imu->pos()(0),
(float)state->_imu->pos()(1),
(float)state->_imu->pos()(2),
(float)state->_imu->quat()(0),
(float)state->_imu->quat()(1),
(float)state->_imu->quat()(2),
(float)state->_imu->quat()(3));
// Finally set the covariance in the message (in the order position then orientation as per ros convention)
std::vector<std::shared_ptr<ov_type::Type>> statevars;
statevars.push_back(state->_imu->pose()->p());
statevars.push_back(state->_imu->pose()->q());
cv::Mat covariance = cv::Mat::eye(6,6, CV_64FC1);
if(this->framesProcessed() == 0)
{
covariance *= 9999;
}
else
{
Eigen::Matrix<double,6,6> covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(),statevars);
for(int r=0; r<6; r++) {
for(int c=0; c<6; c++) {
((double *)covariance.data)[6*r+c] = covariance_posori(r,c);
}
} }
} else
if(!p.isNull())
{
p = p * imuLocalTransform_.inverse();
if(this->getPose().rotation().isIdentity())
{ {
initGravity_ = true; T_left_right = data.stereoCameraModels()[0].stereoTransform().inverse();
this->reset(this->getPose()*p.rotation());
} }
UASSERT(!T_left_right.isNull());
if(previousPose_.isIdentity()) Transform T_imu_right = T_imu_left * T_left_right;
{ Eigen::Matrix4d T_RtoI = T_imu_right.toEigen4d();
previousPose_ = p; Eigen::Matrix<double,7,1> right_eigen;
} right_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_RtoI.block(0,0,3,3).transpose());
right_eigen.block(4,0,3,1) = -T_RtoI.block(0,0,3,3).transpose()*T_RtoI.block(0,3,3,1);
// make it incremental params_->camera_intrinsics.at(1)->set_value(right_calib);
Transform previousPoseInv = previousPose_.inverse(); params_->camera_extrinsics.emplace(1, right_eigen);
t = previousPoseInv*p;
previousPose_ = p;
if(info)
{
info->type = this->getType();
info->reg.covariance = covariance;
// feature map
Transform fixT = this->getPose()*previousPoseInv;
Transform camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse()*this->getPose().inverse();
for (auto &it_per_id : vioManager_->get_features_SLAM())
{
cv::Point3f pt3d;
pt3d.x = it_per_id[0];
pt3d.y = it_per_id[1];
pt3d.z = it_per_id[2];
pt3d = util3d::transformPoint(pt3d, fixT);
info->localMap.insert(std::make_pair(info->localMap.size(), pt3d));
if(this->imagesAlreadyRectified())
{
cv::Point2f pt;
pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
info->reg.inliersIDs.push_back(info->newCorners.size());
info->newCorners.push_back(pt);
}
}
info->features = info->newCorners.size();
info->localMapSize = info->localMap.size();
}
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
} }
params_->init_options.camera_intrinsics = params_->camera_intrinsics;
params_->init_options.camera_extrinsics = params_->camera_extrinsics;
vioManager_ = std::make_unique<ov_msckf::VioManager>(*params_);
} }
} }
else if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
UERROR("OpenVINS doesn't work with RGB-D data, stereo images are required!");
}
else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
{
UERROR("OpenVINS requires stereo images!");
}
else else
{ {
UERROR("OpenVINS requires stereo images (only one stereo camera and should be calibrated)!"); if(!data.imu().empty())
{
ov_core::ImuData message;
message.timestamp = data.stamp();
message.wm << data.imu().angularVelocity().val[0], data.imu().angularVelocity().val[1], data.imu().angularVelocity().val[2];
message.am << data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[1], data.imu().linearAcceleration().val[2];
vioManager_->feed_measurement_imu(message);
}
if(!data.imageRaw().empty())
{
cv::Mat image;
if(data.imageRaw().type() == CV_8UC3)
cv::cvtColor(data.imageRaw(), image, CV_BGR2GRAY);
else if(data.imageRaw().type() == CV_8UC1)
image = data.imageRaw().clone();
else
UFATAL("Not supported color type!");
ov_core::CameraData message;
message.timestamp = data.stamp();
message.sensor_ids.emplace_back(0);
message.images.emplace_back(image);
message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
if(!data.rightRaw().empty())
{
if(data.rightRaw().type() == CV_8UC3)
cv::cvtColor(data.rightRaw(), image, CV_BGR2GRAY);
else if(data.rightRaw().type() == CV_8UC1)
image = data.rightRaw().clone();
else
UFATAL("Not supported color type!");
message.sensor_ids.emplace_back(1);
message.images.emplace_back(image);
message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
}
vioManager_->feed_measurement_camera(message);
if(vioManager_->initialized())
{
std::shared_ptr<ov_msckf::State> state = vioManager_->get_state();
Transform p((float)state->_imu->pos()(0),
(float)state->_imu->pos()(1),
(float)state->_imu->pos()(2),
(float)state->_imu->quat()(0),
(float)state->_imu->quat()(1),
(float)state->_imu->quat()(2),
(float)state->_imu->quat()(3));
if(!p.isNull())
{
p = p * imuLocalTransformInv_;
if(this->getPose().rotation().isIdentity())
{
initGravity_ = true;
this->reset(this->getPose() * p.rotation());
}
if(previousPoseInv_.isIdentity())
previousPoseInv_ = p.inverse();
t = previousPoseInv_ * p;
if(info)
{
info->type = this->getType();
info->reg.covariance = cv::Mat(6,6,CV_64FC1);
std::vector<std::shared_ptr<ov_type::Type>> statevars;
statevars.emplace_back(state->_imu->pose()->p());
statevars.emplace_back(state->_imu->pose()->q());
Eigen::Matrix<double,6,6> covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(state, statevars);
for (int r = 0; r < 6; r++)
{
for (int c = 0; c < 6; c++)
{
((double *)info->reg.covariance.data)[6*r+c] = covariance_posori(r,c);
}
}
Transform fixT = this->getPose() * previousPoseInv_;
Transform camLocalTransformInv;
if(!data.rightRaw().empty())
camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse();
else
camLocalTransformInv = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse();
for (auto &it_per_id : vioManager_->get_features_SLAM())
{
cv::Point3f pt3d(it_per_id[0], it_per_id[1], it_per_id[2]);
pt3d = util3d::transformPoint(pt3d, fixT);
info->localMap.emplace_hint(info->localMap.end(), info->localMap.size(), pt3d);
if(this->imagesAlreadyRectified())
{
cv::Point2f pt;
pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
if(!data.rightRaw().empty())
data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
else
data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
info->reg.inliersIDs.emplace_back(info->newCorners.size());
info->newCorners.emplace_back(pt);
}
}
info->features = info->newCorners.size();
info->localMapSize = info->localMap.size();
}
previousPoseInv_ = p.inverse();
}
}
}
} }
#else #else

42
guilib/src/PreferencesDialog.cpp
View File

@@ -1433,6 +1433,48 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->lineEdit_OdomVinsPath->setObjectName(Parameters::kOdomVINSConfigPath().c_str()); _ui->lineEdit_OdomVinsPath->setObjectName(Parameters::kOdomVINSConfigPath().c_str());
connect(_ui->toolButton_OdomVinsPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSConfigPath())); connect(_ui->toolButton_OdomVinsPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSConfigPath()));
// Odometry OpenVINS
_ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str());
_ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str());
_ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str());
_ui->spinBox_OdomOpenVINSFastThreshold->setObjectName(Parameters::kOdomOpenVINSFastThreshold().c_str());
_ui->spinBox_OdomOpenVINSGridX->setObjectName(Parameters::kOdomOpenVINSGridX().c_str());
_ui->spinBox_OdomOpenVINSGridY->setObjectName(Parameters::kOdomOpenVINSGridY().c_str());
_ui->spinBox_OdomOpenVINSMinPxDist->setObjectName(Parameters::kOdomOpenVINSMinPxDist().c_str());
_ui->doubleSpinBox_OdomOpenVINSKNNRatio->setObjectName(Parameters::kOdomOpenVINSKNNRatio().c_str());
_ui->checkBox_OdomOpenVINSUseFEJ->setObjectName(Parameters::kOdomOpenVINSUseFEJ().c_str());
_ui->comboBox_OdomOpenVINSIntegration->setObjectName(Parameters::kOdomOpenVINSIntegration().c_str());
_ui->spinBox_OdomOpenVINSMaxClones->setObjectName(Parameters::kOdomOpenVINSMaxClones().c_str());
_ui->spinBox_OdomOpenVINSMaxSLAM->setObjectName(Parameters::kOdomOpenVINSMaxSLAM().c_str());
_ui->spinBox_OdomOpenVINSMaxSLAMInUpdate->setObjectName(Parameters::kOdomOpenVINSMaxSLAMInUpdate().c_str());
_ui->spinBox_OdomOpenVINSMaxMSCKFInUpdate->setObjectName(Parameters::kOdomOpenVINSMaxMSCKFInUpdate().c_str());
_ui->comboBox_OdomOpenVINSFeatRepMSCKF->setObjectName(Parameters::kOdomOpenVINSFeatRepMSCKF().c_str());
_ui->comboBox_OdomOpenVINSFeatRepSLAM->setObjectName(Parameters::kOdomOpenVINSFeatRepSLAM().c_str());
_ui->doubleSpinBox_OdomOpenVINSDtSLAMDelay->setObjectName(Parameters::kOdomOpenVINSDtSLAMDelay().c_str());
_ui->doubleSpinBox_OdomOpenVINSGravityMag->setObjectName(Parameters::kOdomOpenVINSGravityMag().c_str());
_ui->doubleSpinBox_OdomOpenVINSInitWindowTime->setObjectName(Parameters::kOdomOpenVINSInitWindowTime().c_str());
_ui->doubleSpinBox_OdomOpenVINSInitIMUThresh->setObjectName(Parameters::kOdomOpenVINSInitIMUThresh().c_str());
_ui->doubleSpinBox_OdomOpenVINSInitMaxDisparity->setObjectName(Parameters::kOdomOpenVINSInitMaxDisparity().c_str());
_ui->spinBox_OdomOpenVINSInitMaxFeatures->setObjectName(Parameters::kOdomOpenVINSInitMaxFeatures().c_str());
_ui->checkBox_OdomOpenVINSTryZUPT->setObjectName(Parameters::kOdomOpenVINSTryZUPT().c_str());
_ui->doubleSpinBox_OdomOpenVINSZUPTChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSZUPTChi2Multiplier().c_str());
_ui->doubleSpinBox_OdomOpenVINSZUPTMaxVelodicy->setObjectName(Parameters::kOdomOpenVINSZUPTMaxVelodicy().c_str());
_ui->doubleSpinBox_OdomOpenVINSZUPTNoiseMultiplier->setObjectName(Parameters::kOdomOpenVINSZUPTNoiseMultiplier().c_str());
_ui->doubleSpinBox_OdomOpenVINSZUPTMaxDisparity->setObjectName(Parameters::kOdomOpenVINSZUPTMaxDisparity().c_str());
_ui->checkBox_OdomOpenVINSZUPTOnlyAtBeginning->setObjectName(Parameters::kOdomOpenVINSZUPTOnlyAtBeginning().c_str());
_ui->doubleSpinBox_OdomOpenVINSAccelerometerNoiseDensity->setObjectName(Parameters::kOdomOpenVINSAccelerometerNoiseDensity().c_str());
_ui->doubleSpinBox_OdomOpenVINSAccelerometerRandomWalk->setObjectName(Parameters::kOdomOpenVINSAccelerometerRandomWalk().c_str());
_ui->doubleSpinBox_OdomOpenVINSGyroscopeNoiseDensity->setObjectName(Parameters::kOdomOpenVINSGyroscopeNoiseDensity().c_str());
_ui->doubleSpinBox_OdomOpenVINSGyroscopeRandomWalk->setObjectName(Parameters::kOdomOpenVINSGyroscopeRandomWalk().c_str());
_ui->doubleSpinBox_OdomOpenVINSUpMSCKFSigmaPx->setObjectName(Parameters::kOdomOpenVINSUpMSCKFSigmaPx().c_str());
_ui->doubleSpinBox_OdomOpenVINSUpMSCKFChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier().c_str());
_ui->doubleSpinBox_OdomOpenVINSUpSLAMSigmaPx->setObjectName(Parameters::kOdomOpenVINSUpSLAMSigmaPx().c_str());
_ui->doubleSpinBox_OdomOpenVINSUpSLAMChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSUpSLAMChi2Multiplier().c_str());
//Stereo //Stereo
_ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str()); _ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str());
_ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str()); _ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str());

1069
guilib/src/ui/preferencesDialog.ui
View File

File diff suppressed because it is too large Load Diff