diff --git a/core/calibration/ImuMagnetometerCalibration.cpp b/core/calibration/ImuMagnetometerCalibration.cpp index 9118017ac..aebf32a97 100644 --- a/core/calibration/ImuMagnetometerCalibration.cpp +++ b/core/calibration/ImuMagnetometerCalibration.cpp @@ -47,11 +47,15 @@ ImuCalibration::ImuCalibration( const Eigen::Vector3d& biasAccel, const Eigen::Matrix3d& rectificationMatrixGyro, const Eigen::Vector3d& biasGyro, - const Sophus::SE3d& T_Device_Imu) + const Sophus::SE3d& T_Device_Imu, + double timeOffsetSecDeviceAccel, + double timeOffsetSecDeviceGyro) : label_(label), accel_(rectificationMatrixAccel, biasAccel), gyro_(rectificationMatrixGyro, biasGyro), - T_Device_Imu_(T_Device_Imu) {} + T_Device_Imu_(T_Device_Imu), + timeOffsetSecDeviceAccel_(timeOffsetSecDeviceAccel), + timeOffsetSecDeviceGyro_(timeOffsetSecDeviceGyro) {} std::string ImuCalibration::getLabel() const { return label_; @@ -84,6 +88,14 @@ LinearRectificationModel3d ImuCalibration::getGyroModel() const { return gyro_; } +double ImuCalibration::getTimeOffsetSecDeviceAccel() const { + return timeOffsetSecDeviceAccel_; +} + +double ImuCalibration::getTimeOffsetSecDeviceGyro() const { + return timeOffsetSecDeviceGyro_; +} + /* MagnetometerCalibration */ MagnetometerCalibration::MagnetometerCalibration( const std::string& label, diff --git a/core/calibration/ImuMagnetometerCalibration.h b/core/calibration/ImuMagnetometerCalibration.h index 63be2f05c..3256a615c 100644 --- a/core/calibration/ImuMagnetometerCalibration.h +++ b/core/calibration/ImuMagnetometerCalibration.h @@ -87,7 +87,9 @@ class ImuCalibration { const Eigen::Vector3d& biasAccel, const Eigen::Matrix3d& rectificationMatrixGyro, const Eigen::Vector3d& biasGyro, - const Sophus::SE3d& T_Device_Imu); + const Sophus::SE3d& T_Device_Imu, + double timeOffsetSecDeviceAccel = 0.0, + double timeOffsetSecDeviceGyro = 0.0); std::string getLabel() const; Sophus::SE3d getT_Device_Imu() const; @@ -120,11 +122,23 @@ class ImuCalibration { */ LinearRectificationModel3d getGyroModel() const; + /** + * @brief return time offset between device and accelerometer + */ + double getTimeOffsetSecDeviceAccel() const; + + /** + * @brief return time offset between device and gyroscope + */ + double getTimeOffsetSecDeviceGyro() const; + private: std::string label_; LinearRectificationModel3d accel_; LinearRectificationModel3d gyro_; Sophus::SE3d T_Device_Imu_; + double timeOffsetSecDeviceAccel_; + double timeOffsetSecDeviceGyro_; }; /** diff --git a/core/calibration/loader/DeviceCalibrationJson.cpp b/core/calibration/loader/DeviceCalibrationJson.cpp index a392b3b6d..820a22db2 100644 --- a/core/calibration/loader/DeviceCalibrationJson.cpp +++ b/core/calibration/loader/DeviceCalibrationJson.cpp @@ -219,26 +219,23 @@ nlohmann::json imuCalibrationToJson(const ImuCalibration& imuCalib) { imuJson["T_Device_Imu"] = json::se3ToJson(imuCalib.getT_Device_Imu()); // Accelerometer calibration - imuJson["Accelerometer"]["TimeOffsetSec_Device_Accel"] = 0; - imuJson["Accelerometer"]["Model"]["Name"] = "UpperTriagonalLinear"; - imuJson["Accelerometer"]["Model"]["RectificationMatrix"] = - json::eigenMatrixToJson(imuCalib.getAccelModel().getRectification()); imuJson["Accelerometer"]["Bias"]["Offset"] = json::eigenVectorToJson(imuCalib.getAccelModel().getBias()); imuJson["Accelerometer"]["Bias"]["Name"] = "Constant"; + imuJson["Accelerometer"]["Model"]["Name"] = "UpperTriagonalLinear"; + imuJson["Accelerometer"]["Model"]["RectificationMatrix"] = + json::eigenMatrixToJson(imuCalib.getAccelModel().getRectification()); + imuJson["Accelerometer"]["TimeOffsetSec_Device_Accel"] = imuCalib.getTimeOffsetSecDeviceAccel(); // Gyroscope calibration imuJson["Gyroscope"]["Bias"]["Name"] = "Constant"; imuJson["Gyroscope"]["Bias"]["Offset"] = json::eigenVectorToJson(imuCalib.getGyroModel().getBias()); + imuJson["Gyroscope"]["Model"]["Name"] = "Linear"; imuJson["Gyroscope"]["Model"]["RectificationMatrix"] = json::eigenMatrixToJson(imuCalib.getGyroModel().getRectification()); - imuJson["Gyroscope"]["Model"]["Name"] = "LinearGSensitivity"; - auto zeroMat = Eigen::Matrix(); - zeroMat.setZero(); - imuJson["Gyroscope"]["Model"]["GSensitivityMatrix"] = json::eigenMatrixToJson(zeroMat); - imuJson["Gyroscope"]["TimeOffsetSec_Device_Gyro"] = 0; + imuJson["Gyroscope"]["TimeOffsetSec_Device_Gyro"] = imuCalib.getTimeOffsetSecDeviceGyro(); return imuJson; } diff --git a/core/calibration/loader/SensorCalibrationJson.cpp b/core/calibration/loader/SensorCalibrationJson.cpp index 414cca5cf..812d711d0 100644 --- a/core/calibration/loader/SensorCalibrationJson.cpp +++ b/core/calibration/loader/SensorCalibrationJson.cpp @@ -86,9 +86,12 @@ ImuCalibration parseImuCalibrationFromJson(const nlohmann::json& json) { const auto& label = json["Label"]; const auto [accelMat, accelBias] = parseRectModelFromJson(json["Accelerometer"]); const auto [gyroMat, gyroBias] = parseRectModelFromJson(json["Gyroscope"]); + const double timeOffsetSecDeviceAccel = static_cast(json["Accelerometer"]["TimeOffsetSec_Device_Accel"]); + const double timeOffsetSecDeviceGyro = static_cast(json["Gyroscope"]["TimeOffsetSec_Device_Gyro"]); const auto T_Device_Imu = se3FromJson(json["T_Device_Imu"]); - return ImuCalibration(label, accelMat, accelBias, gyroMat, gyroBias, T_Device_Imu); + return ImuCalibration(label, accelMat, accelBias, gyroMat, gyroBias, T_Device_Imu, + timeOffsetSecDeviceAccel, timeOffsetSecDeviceGyro); } MagnetometerCalibration parseMagnetometerCalibrationFromJson(