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

View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false, "config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32, "config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false, "config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32, "config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false, "config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32, "config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -23,6 +23,9 @@
"config.vio_use_lm": false, "config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32, "config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -24,6 +24,9 @@
"config.vio_use_lm": false, "config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32, "config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -72,6 +72,10 @@ struct VioConfig {
double vio_lm_lambda_min; double vio_lm_lambda_min;
double vio_lm_lambda_max; 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_std_dev;
double mapper_obs_huber_thresh; double mapper_obs_huber_thresh;
int mapper_detection_num_points; int mapper_detection_num_points;

View File

@ -63,7 +63,7 @@ class KeypointVioEstimator : public VioEstimatorBase,
typedef Eigen::Matrix<double, N, N> MatNN; typedef Eigen::Matrix<double, N, N> MatNN;
typedef Eigen::Matrix<double, N, 3> MatN3; 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 basalt::Calibration<double>& calib,
const VioConfig& config); const VioConfig& config);

View File

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

View File

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

View File

@ -723,7 +723,7 @@ void setup_vio() {
basalt::VioConfig config; basalt::VioConfig config;
config.vio_debug = true; 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(), vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
gt_accel_bias.front()); gt_accel_bias.front());

View File

@ -169,7 +169,7 @@ int main(int argc, char** argv) {
t265_device->image_data_queue = &opt_flow_ptr->input_queue; t265_device->image_data_queue = &opt_flow_ptr->input_queue;
vio = basalt::VioEstimatorFactory::getVioEstimator( 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()); vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
t265_device->imu_data_queue = &vio->imu_data_queue; t265_device->imu_data_queue = &vio->imu_data_queue;

View File

@ -72,6 +72,10 @@ VioConfig::VioConfig() {
vio_lm_lambda_min = 1e-32; vio_lm_lambda_min = 1e-32;
vio_lm_lambda_max = 1e2; 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_std_dev = 0.25;
mapper_obs_huber_thresh = 1.5; mapper_obs_huber_thresh = 1.5;
mapper_detection_num_points = 800; 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_min));
ar(CEREAL_NVP(config.vio_lm_lambda_max)); 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_std_dev));
ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
ar(CEREAL_NVP(config.mapper_detection_num_points)); ar(CEREAL_NVP(config.mapper_detection_num_points));

View File

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

View File

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

View File

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

View File

@ -269,7 +269,7 @@ int main(int argc, char** argv) {
const int64_t start_t_ns = vio_dataset->get_image_timestamps().front(); const int64_t start_t_ns = vio_dataset->get_image_timestamps().front();
{ {
vio = basalt::VioEstimatorFactory::getVioEstimator( 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()); vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
opt_flow_ptr->output_queue = &vio->vision_data_queue; opt_flow_ptr->output_queue = &vio->vision_data_queue;

View File

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