State Initialization with Range Sensor

[goldbattle]

Thanks for open sourcing your code.
I was unable to find the initialization code which leverages the range finder.
Right now it looks like the state is just initialized to all zeros with some hard coded priors.

x/src/x/vio/vio.cpp

Lines 218 to 317 in 2a8fb83

	void VIO::initAtTime(double now) {
	ekf_.lock();
	initialized_ = false;
	vio_updater_.track_manager_.clear();
	vio_updater_.state_manager_.clear();
	// Initial IMU measurement (specific force, velocity)
	Vector3 a_m, w_m;
	a_m << 0.0, 0.0, 9.81;
	w_m << 0.0, 0.0, 0.0;
	// Initial time cannot be 0, which may happen when using simulated Clock time
	// before the first message has been received.
	const double timestamp =
	now > 0.0 ? now : std::numeric_limits<double>::epsilon();
	//////////////////////////////// xEKF INIT ///////////////////////////////////
	// Initial core covariance matrix
	// TODO(jeff) read from params
	const double sigma_dp_x = 0.0;
	const double sigma_dp_y = 0.0;
	const double sigma_dp_z = 0.0;
	const double sigma_dv_x = 0.05;
	const double sigma_dv_y = 0.05;
	const double sigma_dv_z = 0.05;
	const double sigma_dtheta_x = 3.0*M_PI/180.0;
	const double sigma_dtheta_y = 3.0*M_PI/180.0;
	const double sigma_dtheta_z = 3.0*M_PI/180.0;
	const double sigma_dbw_x = 6.0*M_PI/180.0;
	const double sigma_dbw_y = 6.0*M_PI/180.0;
	const double sigma_dbw_z = 6.0*M_PI/180.0;
	const double sigma_dba_x = 0.3;
	const double sigma_dba_y = 0.3;
	const double sigma_dba_z = 0.3;
	const double sigma_dtheta_ic_x = 1.0 * M_PI / 180.0;
	const double sigma_dtheta_ic_y = 1.0 * M_PI / 180.0;
	const double sigma_dtheta_ic_z = 1.0 * M_PI / 180.0;
	const double sigma_dp_ic_x = 0.01;
	const double sigma_dp_ic_y = 0.01;
	const double sigma_dp_ic_z = 0.01;
	const size_t n_poses_state = params_.n_poses_max;
	const size_t n_features_state = params_.n_slam_features_max;
	// Initial vision state estimates and uncertainties are all zero
	const Matrix p_array = Matrix::Zero(n_poses_state * 3, 1);
	const Matrix q_array = Matrix::Zero(n_poses_state * 4, 1);
	const Matrix f_array = Matrix::Zero(n_features_state * 3, 1);
	const Eigen::VectorXd sigma_p_array = Eigen::VectorXd::Zero(n_poses_state * 3);
	const Eigen::VectorXd sigma_q_array = Eigen::VectorXd::Zero(n_poses_state * 3);
	const Eigen::VectorXd sigma_f_array = Eigen::VectorXd::Zero(n_features_state * 3);
	// Construct initial covariance matrix
	const size_t n_err = kSizeCoreErr + n_poses_state * 6 + n_features_state * 3;
	Eigen::VectorXd sigma_diag(n_err);
	sigma_diag << sigma_dp_x, sigma_dp_y, sigma_dp_z,
	sigma_dv_x, sigma_dv_y, sigma_dv_z,
	sigma_dtheta_x, sigma_dtheta_y, sigma_dtheta_z,
	sigma_dbw_x, sigma_dbw_y, sigma_dbw_z,
	sigma_dba_x, sigma_dba_y, sigma_dba_z,
	sigma_dtheta_ic_x, sigma_dtheta_ic_y, sigma_dtheta_ic_z,
	sigma_dp_ic_x, sigma_dp_ic_y, sigma_dp_ic_z,
	sigma_p_array, sigma_q_array, sigma_f_array;
	const Eigen::VectorXd cov_diag = sigma_diag.array() * sigma_diag.array();
	const Matrix cov = cov_diag.asDiagonal();
	// Construct initial state
	const unsigned int dummy_seq = 0;
	State init_state(timestamp,
	dummy_seq,
	params_.p,
	params_.v,
	params_.q,
	params_.b_w,
	params_.b_a,
	p_array,
	q_array,
	f_array,
	cov,
	params_.q_ic,
	params_.p_ic,
	w_m,
	a_m);
	// Try to initialize the filter with initial state input
	try {
	ekf_.initializeFromState(init_state);
	} catch (std::runtime_error& e) {
	std::cerr << "bad input: " << e.what() << std::endl;
	} catch (init_bfr_mismatch) {
	std::cerr << "init_bfr_mismatch: the size of dynamic arrays in the "
	"initialization state match must match the size allocated in "
	"the buffered states." << std::endl;
	}
	ekf_.unlock();
	initialized_ = true;
	}

If this is incorrect, could you point me to the right section of the code? Thanks!

Range-Visual-Inertial Odometry: Scale Observability Without Excitation
Jeff Delaune; David S. Bayard; Roland Brockers
https://ieeexplore.ieee.org/abstract/document/9353193