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

fix calibration

Browse Source
This commit is contained in:
Vladyslav Usenko 2019-07-29 15:02:25 +02:00
parent f48960e1dd
commit 547ed06a13
1 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/device/rs_t265.cpp
View File

@ -236,10 +236,11 @@ std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
Eigen::Matrix<Scalar, 12, 1> gyro_bias_full;
gyro_bias_full << intrinsics.data[0][3], intrinsics.data[1][3],
intrinsics.data[2][3], intrinsics.data[0][0], intrinsics.data[1][0],
intrinsics.data[2][0], intrinsics.data[0][1], intrinsics.data[1][1],
intrinsics.data[2][1], intrinsics.data[0][2], intrinsics.data[1][2],
intrinsics.data[2][2];
intrinsics.data[2][3], intrinsics.data[0][0] - 1.0,
intrinsics.data[1][0], intrinsics.data[2][0], intrinsics.data[0][1],
intrinsics.data[1][1] - 1.0, intrinsics.data[2][1],
intrinsics.data[0][2], intrinsics.data[1][2],
intrinsics.data[2][2] - 1.0;
basalt::CalibGyroBias<Scalar> gyro_bias;
gyro_bias.getParam() = gyro_bias_full;
calib->calib_gyro_bias = gyro_bias;
@ -267,9 +268,10 @@ std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
rs2_motion_device_intrinsic intrinsics = accel.get_motion_intrinsics();
Eigen::Matrix<Scalar, 9, 1> accel_bias_full;
accel_bias_full << intrinsics.data[0][3], intrinsics.data[1][3],
intrinsics.data[2][3], intrinsics.data[0][0], intrinsics.data[1][0],
intrinsics.data[2][0], intrinsics.data[1][1], intrinsics.data[2][1],
intrinsics.data[2][2];
intrinsics.data[2][3], intrinsics.data[0][0] - 1.0,
intrinsics.data[1][0], intrinsics.data[2][0],
intrinsics.data[1][1] - 1.0, intrinsics.data[2][1],
intrinsics.data[2][2] - 1.0;
basalt::CalibAccelBias<Scalar> accel_bias;
accel_bias.getParam() = accel_bias_full;
calib->calib_accel_bias = accel_bias;