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

Use Vec3 for IMU noise. Moved noise parameters from IMU data to calibration.

This commit is contained in:
Vladyslav Usenko 2019-07-19 19:00:57 +02:00
parent 538d021ae1
commit 1d10c35731
15 changed files with 118 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
data/euroc_ds_calib.json
View File

@ -184,10 +184,26 @@
0.0
],
"imu_update_rate": 200.0,
"accel_noise_std": 0.016,
"gyro_noise_std": 0.000282,
"accel_bias_std": 0.001,
"gyro_bias_std": 0.0001,
"accel_noise_std": [
0.016,
0.016,
0.016
],
"gyro_noise_std": [
0.000282,
0.000282,
0.000282
],
"accel_bias_std": [
0.001,
0.001,
0.001
],
"gyro_bias_std": [
0.0001,
0.0001,
0.0001
],
"T_mocap_world": {
"px": 0.0,
"py": 0.0,

8
data/euroc_eucm_calib.json
View File

@ -184,10 +184,10 @@
0.0
],
"imu_update_rate": 200.0,
"accel_noise_std": 0.016,
"gyro_noise_std": 0.000282,
"accel_bias_std": 0.001,
"gyro_bias_std": 0.0001,
"accel_noise_std": [0.016, 0.016, 0.016],
"gyro_noise_std": [0.000282, 0.000282, 0.000282],
"accel_bias_std": [0.001, 0.001, 0.001],
"gyro_bias_std": [0.0001, 0.0001, 0.0001],
"T_mocap_world": {
"px": 0.0,
"py": 0.0,

8
data/tumvi_512_ds_calib.json
View File

@ -80,10 +80,10 @@
0.0
],
"imu_update_rate": 200.0,
"accel_noise_std": 0.016,
"gyro_noise_std": 0.000282,
"accel_bias_std": 0.001,
"gyro_bias_std": 0.0001,
"accel_noise_std": [0.016, 0.016, 0.016],
"gyro_noise_std": [0.000282, 0.000282, 0.000282],
"accel_bias_std": [0.001, 0.001, 0.001],
"gyro_bias_std": [0.0001, 0.0001, 0.0001],
"T_mocap_world": {
"px": 0.0,
"py": 0.0,

8
data/tumvi_512_eucm_calib.json
View File

@ -80,10 +80,10 @@
0.0
],
"imu_update_rate": 200.0,
"accel_noise_std": 0.016,
"gyro_noise_std": 0.000282,
"accel_bias_std": 0.001,
"gyro_bias_std": 0.0001,
"accel_noise_std": [0.016, 0.016, 0.016],
"gyro_noise_std": [0.000282, 0.000282, 0.000282],
"accel_bias_std": [0.001, 0.001, 0.001],
"gyro_bias_std": [0.0001, 0.0001, 0.0001],
"T_mocap_world": {
"px": 0.0,
"py": 0.0,

4
include/basalt/optimization/linearize.h
View File

@ -122,8 +122,8 @@ struct LinearizeBase {
bool opt_imu_scale;
Scalar pose_var_inv;
Scalar gyro_var_inv;
Scalar accel_var_inv;
Vector3 gyro_var_inv;
Vector3 accel_var_inv;
Scalar mocap_var_inv;
Scalar huber_thresh;

60
include/basalt/optimization/spline_linearize.h
View File

@ -235,9 +235,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
// std::cerr << "sline.maxTime() " << spline.maxTime() << std::endl;
// std::cerr << "================================" << std::endl;
const Scalar& accel_var_inv = this->common_data.accel_var_inv;
const Vector3& accel_var_inv = this->common_data.accel_var_inv;
error += accel_var_inv * residual.squaredNorm();
error += residual.transpose() * accel_var_inv.asDiagonal() * residual;
size_t start_bias =
this->common_data.bias_block_offset + ACCEL_BIAS_OFFSET;
@ -255,39 +255,45 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
accum.template addH<POSE_SIZE, POSE_SIZE>(
start_i, start_j,
accel_var_inv * J.d_val_d_knot[i].transpose() *
J.d_val_d_knot[i].transpose() * accel_var_inv.asDiagonal() *
J.d_val_d_knot[j]);
}
accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
start_bias, start_i,
accel_var_inv * J_bias.transpose() * J.d_val_d_knot[i]);
J_bias.transpose() * accel_var_inv.asDiagonal() *
J.d_val_d_knot[i]);
if (this->common_data.opt_g) {
accum.template addH<G_SIZE, POSE_SIZE>(
start_g, start_i,
accel_var_inv * J_g.transpose() * J.d_val_d_knot[i]);
J_g.transpose() * accel_var_inv.asDiagonal() * J.d_val_d_knot[i]);
}
accum.template addB<POSE_SIZE>(
start_i, accel_var_inv * J.d_val_d_knot[i].transpose() * residual);
accum.template addB<POSE_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
accel_var_inv.asDiagonal() *
residual);
}
accum.template addH<ACCEL_BIAS_SIZE, ACCEL_BIAS_SIZE>(
start_bias, start_bias, accel_var_inv * J_bias.transpose() * J_bias);
start_bias, start_bias,
J_bias.transpose() * accel_var_inv.asDiagonal() * J_bias);
if (this->common_data.opt_g) {
accum.template addH<G_SIZE, ACCEL_BIAS_SIZE>(
start_g, start_bias, accel_var_inv * J_g.transpose() * J_bias);
start_g, start_bias,
J_g.transpose() * accel_var_inv.asDiagonal() * J_bias);
accum.template addH<G_SIZE, G_SIZE>(
start_g, start_g, accel_var_inv * J_g.transpose() * J_g);
start_g, start_g,
J_g.transpose() * accel_var_inv.asDiagonal() * J_g);
}
accum.template addB<ACCEL_BIAS_SIZE>(
start_bias, accel_var_inv * J_bias.transpose() * residual);
start_bias,
J_bias.transpose() * accel_var_inv.asDiagonal() * residual);
if (this->common_data.opt_g) {
accum.template addB<G_SIZE>(start_g,
accel_var_inv * J_g.transpose() * residual);
accum.template addB<G_SIZE>(
start_g, J_g.transpose() * accel_var_inv.asDiagonal() * residual);
}
}
}
@ -305,7 +311,7 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
BASALT_ASSERT(t_ns >= spline->minTimeNs());
BASALT_ASSERT(t_ns <= spline->maxTimeNs());
const Scalar& gyro_var_inv = this->common_data.gyro_var_inv;
const Vector3& gyro_var_inv = this->common_data.gyro_var_inv;
Vector3 residual = spline->gyroResidual(
t_ns, pm.data, this->common_data.calibration->calib_gyro_bias, &J,
@ -323,7 +329,7 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
// std::cerr << "pm.data " << pm.data.transpose() << std::endl;
// std::cerr << "t_ns " << t_ns << std::endl;
error += gyro_var_inv * residual.squaredNorm();
error += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
size_t start_bias =
this->common_data.bias_block_offset + GYRO_BIAS_OFFSET;
@ -344,19 +350,23 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
accum.template addH<ROT_SIZE, ROT_SIZE>(
start_i, start_j,
gyro_var_inv * J.d_val_d_knot[i].transpose() * J.d_val_d_knot[j]);
J.d_val_d_knot[i].transpose() * gyro_var_inv.asDiagonal() *
J.d_val_d_knot[j]);
}
accum.template addH<GYRO_BIAS_SIZE, ROT_SIZE>(
start_bias, start_i,
gyro_var_inv * J_bias.transpose() * J.d_val_d_knot[i]);
accum.template addB<ROT_SIZE>(
start_i, gyro_var_inv * J.d_val_d_knot[i].transpose() * residual);
J_bias.transpose() * gyro_var_inv.asDiagonal() * J.d_val_d_knot[i]);
accum.template addB<ROT_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
gyro_var_inv.asDiagonal() *
residual);
}
accum.template addH<GYRO_BIAS_SIZE, GYRO_BIAS_SIZE>(
start_bias, start_bias, gyro_var_inv * J_bias.transpose() * J_bias);
start_bias, start_bias,
J_bias.transpose() * gyro_var_inv.asDiagonal() * J_bias);
accum.template addB<GYRO_BIAS_SIZE>(
start_bias, gyro_var_inv * J_bias.transpose() * residual);
start_bias,
J_bias.transpose() * gyro_var_inv.asDiagonal() * residual);
}
}
@ -732,9 +742,9 @@ struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
t, pm.data, this->common_data.calibration->calib_accel_bias,
*(this->common_data.g));
const Scalar& accel_var_inv = this->common_data.accel_var_inv;
const Vector3& accel_var_inv = this->common_data.accel_var_inv;
error += accel_var_inv * residual.squaredNorm();
error += residual.transpose() * accel_var_inv.asDiagonal() * residual;
}
}
@ -748,12 +758,12 @@ struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
BASALT_ASSERT(t_ns >= spline->minTimeNs());
BASALT_ASSERT(t_ns <= spline->maxTimeNs());
const Scalar& gyro_var_inv = this->common_data.gyro_var_inv;
const Vector3& gyro_var_inv = this->common_data.gyro_var_inv;
Vector3 residual = spline->gyroResidual(
t_ns, pm.data, this->common_data.calibration->calib_gyro_bias);
error += gyro_var_inv * residual.squaredNorm();
error += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
}
}

6
include/basalt/optimization/spline_optimize.h
View File

@ -324,8 +324,10 @@ class SplineOptimization {
ccd.opt_g = true;
ccd.pose_var_inv = pose_var_inv;
ccd.gyro_var_inv = 1.0 / (calib->gyro_noise_std * calib->gyro_noise_std);
ccd.accel_var_inv = 1.0 / (calib->accel_noise_std * calib->accel_noise_std);
ccd.gyro_var_inv =
calib->dicreete_time_gyro_noise_std().array().square().inverse();
ccd.accel_var_inv =
calib->dicreete_time_accel_noise_std().array().square().inverse();
ccd.mocap_var_inv = pose_var_inv;
}

8
src/calibration/cam_imu_calib.cpp
View File

@ -651,10 +651,10 @@ void CamImuCalib::loadDataset() {
calib_opt->loadCalib(cache_path);
calib_opt->calib->accel_noise_std = imu_noise[0];
calib_opt->calib->gyro_noise_std = imu_noise[1];
calib_opt->calib->accel_bias_std = imu_noise[2];
calib_opt->calib->gyro_bias_std = imu_noise[3];
calib_opt->calib->accel_noise_std.setConstant(imu_noise[0]);
calib_opt->calib->gyro_noise_std.setConstant(imu_noise[1]);
calib_opt->calib->accel_bias_std.setConstant(imu_noise[2]);
calib_opt->calib->gyro_bias_std.setConstant(imu_noise[3]);
}
calib_opt->resetMocapCalib();

35
src/device/rs_t265.cpp
View File

@ -139,13 +139,6 @@ void RsT265Device::start() {
data->accel = accel_interpolated;
data->gyro = gyro_data.data;
const double accel_noise_std =
calib->dicreete_time_accel_noise_std();
const double gyro_noise_std = calib->dicreete_time_gyro_noise_std();
data->accel_cov.setConstant(accel_noise_std * accel_noise_std);
data->gyro_cov.setConstant(gyro_noise_std * gyro_noise_std);
if (imu_data_queue) imu_data_queue->push(data);
}
@ -253,13 +246,15 @@ std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
// std::cout << "Gyro Bias\n" << gyro_bias_full << std::endl;
calib->gyro_noise_std = std::sqrt(std::max(
std::max(intrinsics.noise_variances[0], intrinsics.noise_variances[1]),
intrinsics.noise_variances[2]));
calib->gyro_noise_std = Eigen::Vector3d(intrinsics.noise_variances[0],
intrinsics.noise_variances[1],
intrinsics.noise_variances[2])
.cwiseSqrt();
calib->gyro_bias_std = std::sqrt(std::max(
std::max(intrinsics.bias_variances[0], intrinsics.bias_variances[1]),
intrinsics.bias_variances[2]));
calib->gyro_bias_std = Eigen::Vector3d(intrinsics.bias_variances[0],
intrinsics.bias_variances[1],
intrinsics.bias_variances[2])
.cwiseSqrt();
// std::cout << "Gyro noise var: " << intrinsics.noise_variances[0]
// << " bias var: " << intrinsics.bias_variances[0] << std::endl;
@ -281,13 +276,15 @@ std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
// std::cout << "Gyro Bias\n" << accel_bias_full << std::endl;
calib->accel_noise_std = sqrt(std::max(
std::max(intrinsics.noise_variances[0], intrinsics.noise_variances[1]),
intrinsics.noise_variances[2]));
calib->accel_noise_std = Eigen::Vector3d(intrinsics.noise_variances[0],
intrinsics.noise_variances[1],
intrinsics.noise_variances[2])
.cwiseSqrt();
calib->accel_bias_std = sqrt(std::max(
std::max(intrinsics.bias_variances[0], intrinsics.bias_variances[1]),
intrinsics.bias_variances[2]));
calib->accel_bias_std = Eigen::Vector3d(intrinsics.bias_variances[0],
intrinsics.bias_variances[1],
intrinsics.bias_variances[2])
.cwiseSqrt();
// std::cout << "Accel noise var: " << intrinsics.noise_variances[0]
// << " bias var: " << intrinsics.bias_variances[0] << std::endl;

14
src/mapper_sim_naive.cpp
View File

@ -174,15 +174,6 @@ int main(int argc, char** argv) {
load_data(cam_calib_path);
std::normal_distribution<> obs_noise_dist{0, obs_std_dev};
std::normal_distribution<> gyro_noise_dist{
0, calib.dicreete_time_gyro_noise_std()};
std::normal_distribution<> accel_noise_dist{
0, calib.dicreete_time_accel_noise_std()};
std::normal_distribution<> gyro_bias_dist{0, calib.gyro_bias_std};
std::normal_distribution<> accel_bias_dist{0, calib.accel_bias_std};
gen_data();
setup_vio();
@ -198,11 +189,6 @@ int main(int argc, char** argv) {
data->accel = noisy_accel[i];
data->gyro = noisy_gyro[i];
data->accel_cov.setConstant(calib.dicreete_time_accel_noise_std() *
calib.dicreete_time_accel_noise_std());
data->gyro_cov.setConstant(calib.dicreete_time_gyro_noise_std() *
calib.dicreete_time_gyro_noise_std());
vio->addIMUToQueue(data);
}

15
src/vi_estimator/keypoint_vio.cpp
View File

@ -75,10 +75,8 @@ KeypointVioEstimator::KeypointVioEstimator(
std::cout << "marg_H\n" << marg_H << std::endl;
gyro_bias_weight.setConstant(1.0 /
(calib.gyro_bias_std * calib.gyro_bias_std));
accel_bias_weight.setConstant(1.0 /
(calib.accel_bias_std * calib.accel_bias_std));
gyro_bias_weight = calib.gyro_bias_std.array().square().inverse();
accel_bias_weight = calib.accel_bias_std.array().square().inverse();
max_states = std::numeric_limits<int>::max();
max_kfs = std::numeric_limits<int>::max();
@ -114,6 +112,10 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
OpticalFlowResult::Ptr prev_frame, curr_frame;
IntegratedImuMeasurement::Ptr meas;
Eigen::Vector3d accel_cov, gyro_cov;
accel_cov = calib.dicreete_time_accel_noise_std().array().square();
gyro_cov = calib.dicreete_time_gyro_noise_std().array().square();
ImuData::Ptr data;
imu_data_queue.pop(data);
@ -165,7 +167,8 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
}
while (data->t_ns <= curr_frame->t_ns) {
meas->integrate(*data);
meas->integrate(*data, calib.dicreete_time_accel_noise_std(),
calib.dicreete_time_gyro_noise_std());
imu_data_queue.pop(data);
if (!data.get()) break;
}
@ -173,7 +176,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) {
int64_t tmp = data->t_ns;
data->t_ns = curr_frame->t_ns;
meas->integrate(*data);
meas->integrate(*data, accel_cov, gyro_cov);
data->t_ns = tmp;
}
}

6
src/vio.cpp
View File

@ -165,12 +165,6 @@ void feed_imu() {
data->accel = vio_dataset->get_accel_data()[i].data;
data->gyro = vio_dataset->get_gyro_data()[i].data;
const double accel_noise_std = calib.dicreete_time_accel_noise_std();
const double gyro_noise_std = calib.dicreete_time_gyro_noise_std();
data->accel_cov.setConstant(accel_noise_std * accel_noise_std);
data->gyro_cov.setConstant(gyro_noise_std * gyro_noise_std);
vio->imu_data_queue.push(data);
}
vio->imu_data_queue.push(nullptr);

13
src/vio_sim.cpp
View File

@ -209,11 +209,6 @@ int main(int argc, char** argv) {
data->accel = noisy_accel[i];
data->gyro = noisy_gyro[i];
data->accel_cov.setConstant(calib.dicreete_time_accel_noise_std() *
calib.dicreete_time_accel_noise_std());
data->gyro_cov.setConstant(calib.dicreete_time_gyro_noise_std() *
calib.dicreete_time_gyro_noise_std());
vio->addIMUToQueue(data);
}
@ -600,12 +595,12 @@ void compute_projections() {
void gen_data() {
std::normal_distribution<> gyro_noise_dist{
0, calib.dicreete_time_gyro_noise_std()};
0, calib.dicreete_time_gyro_noise_std()[0]};
std::normal_distribution<> accel_noise_dist{
0, calib.dicreete_time_accel_noise_std()};
0, calib.dicreete_time_accel_noise_std()[0]};
std::normal_distribution<> gyro_bias_dist{0, calib.gyro_bias_std};
std::normal_distribution<> accel_bias_dist{0, calib.accel_bias_std};
std::normal_distribution<> gyro_bias_dist{0, calib.gyro_bias_std[0]};
std::normal_distribution<> accel_bias_dist{0, calib.accel_bias_std[0]};
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
images.emplace_back();

23
test/src/test_vio.cpp
View File

@ -41,6 +41,10 @@ TEST(VioTestSuite, ImuNullspace2Test) {
state0.bias_gyro = bg;
state0.bias_accel = ba;
Eigen::Vector3d accel_cov, gyro_cov;
accel_cov.setConstant(accel_std_dev * accel_std_dev);
gyro_cov.setConstant(gyro_std_dev * gyro_std_dev);
int64_t dt_ns = 1e7;
for (int64_t t_ns = dt_ns / 2;
t_ns < int64_t(1e8); // gt_spline.maxTimeNs() - int64_t(1e9);
@ -63,12 +67,9 @@ TEST(VioTestSuite, ImuNullspace2Test) {
data.gyro[1] += gyro_noise_dist(gen);
data.gyro[2] += gyro_noise_dist(gen);
data.accel_cov.setConstant(accel_std_dev * accel_std_dev);
data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev);
data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval;
imu_meas.integrate(data);
imu_meas.integrate(data, accel_cov, gyro_cov);
}
state1.t_ns = imu_meas.get_dt_ns();
@ -166,6 +167,10 @@ TEST(VioTestSuite, ImuNullspace3Test) {
state0.bias_gyro = bg;
state0.bias_accel = ba;
Eigen::Vector3d accel_cov, gyro_cov;
accel_cov.setConstant(accel_std_dev * accel_std_dev);
gyro_cov.setConstant(gyro_std_dev * gyro_std_dev);
int64_t dt_ns = 1e7;
for (int64_t t_ns = dt_ns / 2;
t_ns < int64_t(1e9); // gt_spline.maxTimeNs() - int64_t(1e9);
@ -188,12 +193,9 @@ TEST(VioTestSuite, ImuNullspace3Test) {
data.gyro[1] += gyro_noise_dist(gen);
data.gyro[2] += gyro_noise_dist(gen);
data.accel_cov.setConstant(accel_std_dev * accel_std_dev);
data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev);
data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval;
imu_meas1.integrate(data);
imu_meas1.integrate(data, accel_cov, gyro_cov);
}
basalt::IntegratedImuMeasurement imu_meas2(imu_meas1.get_dt_ns(), bg, ba);
@ -218,12 +220,9 @@ TEST(VioTestSuite, ImuNullspace3Test) {
data.gyro[1] += gyro_noise_dist(gen);
data.gyro[2] += gyro_noise_dist(gen);
data.accel_cov.setConstant(accel_std_dev * accel_std_dev);
data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev);
data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval;
imu_meas2.integrate(data);
imu_meas2.integrate(data, accel_cov, gyro_cov);
}
state1.t_ns = imu_meas1.get_dt_ns();

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit 62ad5cbac6bc8c906eeb9c8dad2f2581c1576fec
Subproject commit d169d697f8902ca684e41887eee3bedf91176e56