ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added initialization parameters to config files

This commit is contained in:
Vladyslav Usenko 2019-08-27 15:52:19 +02:00
parent 65808d6f0f
commit 77100b99f2
17 changed files with 44 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
data/euroc_config.json
View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.vio_init_pose_weight": 1e8,
"config.vio_init_ba_weight": 1e1,
"config.vio_init_bg_weight": 1e2,
"config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5,

3
data/euroc_config_no_factors.json
View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.vio_init_pose_weight": 1e8,
"config.vio_init_ba_weight": 1e1,
"config.vio_init_bg_weight": 1e2,
"config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5,

3
data/euroc_config_no_weights.json
View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.vio_init_pose_weight": 1e8,
"config.vio_init_ba_weight": 1e1,
"config.vio_init_bg_weight": 1e2,
"config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5,

3
data/kitti_config.json
View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.vio_init_pose_weight": 1e8,
"config.vio_init_ba_weight": 1e1,
"config.vio_init_bg_weight": 1e2,
"config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5,

3
data/tumvi_512_config.json
View File

@ -24,6 +24,9 @@
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.vio_init_pose_weight": 1e8,
"config.vio_init_ba_weight": 1e1,
"config.vio_init_bg_weight": 1e2,
"config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5,

4
include/basalt/utils/vio_config.h
View File

@ -72,6 +72,10 @@ struct VioConfig {
double vio_lm_lambda_min;
double vio_lm_lambda_max;
double vio_init_pose_weight;
double vio_init_ba_weight;
double vio_init_bg_weight;
double mapper_obs_std_dev;
double mapper_obs_huber_thresh;
int mapper_detection_num_points;

2
include/basalt/vi_estimator/keypoint_vio.h
View File

@ -63,7 +63,7 @@ class KeypointVioEstimator : public VioEstimatorBase,
typedef Eigen::Matrix<double, N, N> MatNN;
typedef Eigen::Matrix<double, N, 3> MatN3;
KeypointVioEstimator(double int_std_dev, const Eigen::Vector3d& g,
KeypointVioEstimator(const Eigen::Vector3d& g,
const basalt::Calibration<double>& calib,
const VioConfig& config);

3
include/basalt/vi_estimator/keypoint_vo.h
View File

@ -63,8 +63,7 @@ class KeypointVoEstimator : public VioEstimatorBase,
typedef Eigen::Matrix<double, N, N> MatNN;
typedef Eigen::Matrix<double, N, 3> MatN3;
KeypointVoEstimator(double int_std_dev,
const basalt::Calibration<double>& calib,
KeypointVoEstimator(const basalt::Calibration<double>& calib,
const VioConfig& config);
void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,

1
include/basalt/vi_estimator/vio_estimator.h
View File

@ -100,7 +100,6 @@ class VioEstimatorFactory {
public:
static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config,
const Calibration<double>& cam,
double int_std_dev,
const Eigen::Vector3d& g,
bool use_imu);
};

2
src/mapper_sim_naive.cpp
View File

@ -723,7 +723,7 @@ void setup_vio() {
basalt::VioConfig config;
config.vio_debug = true;
vio.reset(new basalt::KeypointVioEstimator(0.0001, g, calib, config));
vio.reset(new basalt::KeypointVioEstimator(g, calib, config));
vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
gt_accel_bias.front());

2
src/rs_t265_vio.cpp
View File

@ -169,7 +169,7 @@ int main(int argc, char** argv) {
t265_device->image_data_queue = &opt_flow_ptr->input_queue;
vio = basalt::VioEstimatorFactory::getVioEstimator(
vio_config, calib, 0.0001, basalt::constants::g, true);
vio_config, calib, basalt::constants::g, true);
vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
t265_device->imu_data_queue = &vio->imu_data_queue;

8
src/utils/vio_config.cpp
View File

@ -72,6 +72,10 @@ VioConfig::VioConfig() {
vio_lm_lambda_min = 1e-32;
vio_lm_lambda_max = 1e2;
vio_init_pose_weight = 1e8;
vio_init_ba_weight = 1e1;
vio_init_bg_weight = 1e2;
mapper_obs_std_dev = 0.25;
mapper_obs_huber_thresh = 1.5;
mapper_detection_num_points = 800;
@ -145,6 +149,10 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
ar(CEREAL_NVP(config.vio_lm_lambda_min));
ar(CEREAL_NVP(config.vio_lm_lambda_max));
ar(CEREAL_NVP(config.vio_init_pose_weight));
ar(CEREAL_NVP(config.vio_init_ba_weight));
ar(CEREAL_NVP(config.vio_init_bg_weight));
ar(CEREAL_NVP(config.mapper_obs_std_dev));
ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
ar(CEREAL_NVP(config.mapper_detection_num_points));

14
src/vi_estimator/keypoint_vio.cpp
View File

@ -47,8 +47,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace basalt {
KeypointVioEstimator::KeypointVioEstimator(
double int_std_dev, const Eigen::Vector3d& g,
const basalt::Calibration<double>& calib, const VioConfig& config)
const Eigen::Vector3d& g, const basalt::Calibration<double>& calib,
const VioConfig& config)
: take_kf(true),
frames_after_kf(0),
g(g),
@ -66,16 +66,14 @@ KeypointVioEstimator::KeypointVioEstimator(
marg_H.setZero(POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE);
marg_b.setZero(POSE_VEL_BIAS_SIZE);
double prior_weight = 1.0 / (int_std_dev * int_std_dev);
// prior on position
marg_H.diagonal().head<3>().setConstant(prior_weight);
marg_H.diagonal().head<3>().setConstant(config.vio_init_pose_weight);
// prior on yaw
marg_H(5, 5) = prior_weight;
marg_H(5, 5) = config.vio_init_pose_weight;
// small prior to avoid jumps in bias
marg_H.diagonal().segment<3>(9).array() = 1e2;
marg_H.diagonal().segment<3>(12).array() = 1e3;
marg_H.diagonal().segment<3>(9).array() = config.vio_init_ba_weight;
marg_H.diagonal().segment<3>(12).array() = config.vio_init_bg_weight;
std::cout << "marg_H\n" << marg_H << std::endl;

7
src/vi_estimator/keypoint_vo.cpp
View File

@ -47,8 +47,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace basalt {
KeypointVoEstimator::KeypointVoEstimator(
double int_std_dev, const basalt::Calibration<double>& calib,
const VioConfig& config)
const basalt::Calibration<double>& calib, const VioConfig& config)
: take_kf(true),
frames_after_kf(0),
initialized(false),
@ -65,10 +64,8 @@ KeypointVoEstimator::KeypointVoEstimator(
marg_H.setZero(POSE_SIZE, POSE_SIZE);
marg_b.setZero(POSE_SIZE);
double prior_weight = 1.0 / (int_std_dev * int_std_dev);
// prior on pose
marg_H.diagonal().setConstant(prior_weight);
marg_H.diagonal().setConstant(config.vio_init_pose_weight);
std::cout << "marg_H\n" << marg_H << std::endl;

6
src/vi_estimator/vio_estimator.cpp
View File

@ -41,14 +41,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace basalt {
VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator(
const VioConfig& config, const Calibration<double>& cam, double int_std_dev,
const VioConfig& config, const Calibration<double>& cam,
const Eigen::Vector3d& g, bool use_imu) {
VioEstimatorBase::Ptr res;
if (use_imu) {
res.reset(new KeypointVioEstimator(int_std_dev, g, cam, config));
res.reset(new KeypointVioEstimator(g, cam, config));
} else {
res.reset(new KeypointVoEstimator(int_std_dev, cam, config));
res.reset(new KeypointVoEstimator(cam, config));
}
return res;

2
src/vio.cpp
View File

@ -269,7 +269,7 @@ int main(int argc, char** argv) {
const int64_t start_t_ns = vio_dataset->get_image_timestamps().front();
{
vio = basalt::VioEstimatorFactory::getVioEstimator(
vio_config, calib, 0.0001, basalt::constants::g, use_imu);
vio_config, calib, basalt::constants::g, use_imu);
vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
opt_flow_ptr->output_queue = &vio->vision_data_queue;

3
src/vio_sim.cpp
View File

@ -878,9 +878,8 @@ void setup_vio() {
basalt::VioConfig config;
// vio.reset(new basalt::KeypointVioEstimator(0.0001, g, calib, config));
vio = basalt::VioEstimatorFactory::getVioEstimator(
config, calib, 0.0001, basalt::constants::g, use_imu);
config, calib, basalt::constants::g, use_imu);
vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
gt_accel_bias.front());