From 703bcfad331a7a8e878b3bca62784d046f337d42 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 25 Sep 2025 17:24:52 +0300 Subject: [PATCH 01/12] Added support for Ouster scans with uint16_t ring fields. Signed-off-by: Martin Pecka --- docs/details/config_template_note.md | 3 +- docs/details/sensor_type.md | 2 +- include/sensor/lidar_data_loader.h | 12 ++++++ include/sensor/sensor_model.h | 2 + include/util/cloud_define.hpp | 22 +++++++++++ src/sensor/lidar_data_loader.cpp | 57 ++++++++++++++++++++++++++++ src/sensor/sensor_model.cpp | 5 ++- 7 files changed, 100 insertions(+), 3 deletions(-) diff --git a/docs/details/config_template_note.md b/docs/details/config_template_note.md index 06c6252..116ebf7 100644 --- a/docs/details/config_template_note.md +++ b/docs/details/config_template_note.md @@ -68,10 +68,11 @@ Configor: Type: "AWR1843BOOST_CUSTOM" Weight: 10.0 # 1. Velodyne LiDARs: VLP_16_PACKET, VLP_POINTS - # 2. Ouster LiDARs: OUSTER_POINTS + # 2. Ouster LiDARs: OUSTER_POINTS (Ouster pointcloud with uint8_t ring field) # 3. Hesai Pandar XT LiDARs: PANDAR_XT_POINTS # 4. Livox LiDARs: LIVOX_CUSTOM (the official 'xfer_format'=1, mid-360 and avia is recommend) # 5. Robosense LiDARs: RSLIDAR_POINTS (tested on RS-Helios-16P) + # 6. Ouster LiDARs: OUSTER_POINTS_RING_16 (Ouster pointcloud with uint16_t ring field) LiDARTopics: # if no LiDAR is integrated in your sensor suite, just comment out the following key items - key: "/lidar0/scan" diff --git a/docs/details/sensor_type.md b/docs/details/sensor_type.md index dd5af64..a01ba57 100644 --- a/docs/details/sensor_type.md +++ b/docs/details/sensor_type.md @@ -27,7 +27,7 @@ Currently, the following types of sensor are supported in `iKalibr`: + **LiDAR** type: corresponding type definition can be found [here](https://github.com/Unsigned-Long/iKalibr/blob/master/include/sensor/sensor_model.h#L109-L122). The type of LiDAR is more prone to error than others, most of the time error happens when decoding time stamps of points if the wrong LiDAR type is passed in `iKalibr`. + *Velodyne LiDARs*: `VLP_16_PACKET`, `VLP_POINTS`. - + *Ouster LiDARs*: `OUSTER_POINTS`. + + *Ouster LiDARs*: `OUSTER_POINTS`, `OUSTER_POINTS_RING_16`. + *Hesai Pandar XT LiDARs*: `PANDAR_XT_POINTS`. diff --git a/include/sensor/lidar_data_loader.h b/include/sensor/lidar_data_loader.h index c6a8aff..2d54abb 100644 --- a/include/sensor/lidar_data_loader.h +++ b/include/sensor/lidar_data_loader.h @@ -188,6 +188,18 @@ class OusterLiDAR : public LiDARDataLoader { LiDARFrame::Ptr UnpackScan(const rosbag::MessageInstance &msgInstance) override; }; +class OusterRing16LiDAR : public LiDARDataLoader { +public: + using Ptr = std::shared_ptr; + +public: + explicit OusterRing16LiDAR(LidarModelType lidarModel); + + static OusterRing16LiDAR::Ptr Create(LidarModelType lidarModel); + + LiDARFrame::Ptr UnpackScan(const rosbag::MessageInstance &msgInstance) override; +}; + class PandarXTLiDAR : public LiDARDataLoader { public: using Ptr = std::shared_ptr; diff --git a/include/sensor/sensor_model.h b/include/sensor/sensor_model.h index d383157..1cff23b 100644 --- a/include/sensor/sensor_model.h +++ b/include/sensor/sensor_model.h @@ -118,6 +118,8 @@ struct LidarModel { LIVOX_CUSTOM, RSLIDAR_POINTS, + + OUSTER_POINTS_RING_16, }; static std::string UnsupportedLiDARModelMsg(const std::string &modelStr); diff --git a/include/util/cloud_define.hpp b/include/util/cloud_define.hpp index 9b40931..28fb4bb 100644 --- a/include/util/cloud_define.hpp +++ b/include/util/cloud_define.hpp @@ -112,6 +112,25 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR8Y, (std::uint8_t, ring, ring) (std::uint32_t, t, t)) +// OUSTER lidar with uint16_t ring +struct EIGEN_ALIGN16 PointXYZIR16Y { + PCL_ADD_POINT4D; // quad-word XYZ + float intensity; // laser intensity reading + std::uint16_t ring; // laser ring number + std::uint32_t t; + float range; + + PCL_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment +}; + +POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR16Y, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) + (std::uint32_t, t, t)) + struct EIGEN_ALIGN16 PointXYZITR { PCL_ADD_POINT4D; float intensity; @@ -147,6 +166,9 @@ using PosITPointCloud = pcl::PointCloud; using OusterPoint = PointXYZIR8Y; using OusterPointCloud = pcl::PointCloud; +using OusterRing16Point = PointXYZIR16Y; +using OusterRing16PointCloud = pcl::PointCloud; + using ColorPoint = pcl::PointXYZRGBA; using ColorPointCloud = pcl::PointCloud; diff --git a/src/sensor/lidar_data_loader.cpp b/src/sensor/lidar_data_loader.cpp index d38bae1..625b9af 100644 --- a/src/sensor/lidar_data_loader.cpp +++ b/src/sensor/lidar_data_loader.cpp @@ -79,6 +79,9 @@ LiDARDataLoader::Ptr LiDARDataLoader::GetLoader(const std::string &lidarModelStr case LidarModelType::RSLIDAR_POINTS: dataLoader = RSLIDAR_POINTS::Create(lidarModel); break; + case LidarModelType::OUSTER_POINTS_RING_16: + dataLoader = OusterRing16LiDAR::Create(lidarModel); + break; default: throw Status(Status::WARNING, LidarModel::UnsupportedLiDARModelMsg(lidarModelStr)); } @@ -394,6 +397,60 @@ LiDARFrame::Ptr OusterLiDAR::UnpackScan(const rosbag::MessageInstance &msgInstan return LiDARFrame::Create(timebase, cloud); } +// ------------------------------ +// OusterLiDAR with uint16_t ring +// ------------------------------ +OusterRing16LiDAR::OusterRing16LiDAR(LidarModelType lidarModel) + : LiDARDataLoader(lidarModel) {} + +OusterRing16LiDAR::Ptr OusterRing16LiDAR::Create(LidarModelType lidarModel) { + return std::make_shared(lidarModel); +} + +LiDARFrame::Ptr OusterRing16LiDAR::UnpackScan(const rosbag::MessageInstance &msgInstance) { + sensor_msgs::PointCloud2::ConstPtr lidarMsg = + msgInstance.instantiate(); + + CheckMessage(lidarMsg); + + OusterRing16PointCloud pcIn; + pcl::fromROSMsg(*lidarMsg, pcIn); + + if (lidarMsg->header.stamp.isZero()) { + Status(Status::WARNING, "lidar scan with zero timestamp exists!!!"); + } + double timebase = lidarMsg->header.stamp.toSec(); + + IKalibrPointCloud::Ptr cloud(new IKalibrPointCloud()); + cloud->is_dense = false; + cloud->resize(pcIn.size()); + + std::size_t j = 0; + for (const auto &src : pcIn) { + if (IS_POS_NAN(src)) { + continue; + } + + double depth = sqrt(src.x * src.x + src.y * src.y + src.z * src.z); + + if (depth > 60 || depth < 1) { + continue; + } else { + IKalibrPoint dstPoint; + dstPoint.x = src.x; + dstPoint.y = src.y; + dstPoint.z = src.z; + dstPoint.timestamp = timebase + static_cast(src.t) * 1E-9; + // attention: use 'PointXYZT' as 'IKalibrPoint' rather than 'PointXYZIT' here + // dstPoint.intensity = src.intensity; + cloud->at(j++) = dstPoint; + } + } + cloud->resize(j); + + return LiDARFrame::Create(timebase, cloud); +} + // ------------- // PandarXTLiDAR // ------------- diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_model.cpp index 807d871..69a071f 100644 --- a/src/sensor/sensor_model.cpp +++ b/src/sensor/sensor_model.cpp @@ -113,8 +113,11 @@ std::string LidarModel::UnsupportedLiDARModelMsg(const std::string &modelStr) { "3. Hesai Pandar XT LiDARs: PANDAR_XT_POINTS\n" "4. Livox LiDARs: LIVOX_CUSTOM (the official 'xfer_format'=1, " "mid-360 and avia is recommend)\n" + "5. Robosense LiDARs: RSLIDAR_POINTS (tested on RS-Helios-16P)\n" + "6. Ouster LiDARs: OUSTER_POINTS_RING_16 (Ouster pointcloud with " + "uint16_t ring field)" "...\n" - "If you need to use other IMU types, " + "If you need to use other LiDAR types, " "please 'Issues' us on the profile of the github repository.", modelStr); } From 6c1bcc07040b9a89d0b848c1e139f1a4cf2cf277 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Sun, 5 Oct 2025 10:55:01 +0200 Subject: [PATCH 02/12] Fixed invalid spdlog prints. --- src/solver/calib_solver_init_sen_inertial_align_impl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp index 6b37aa2..2361176 100644 --- a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp +++ b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp @@ -114,7 +114,7 @@ void CalibSolver::InitSensorInertialAlign() const { std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetLiDARAvgFrequency(lidarTopic))); spdlog::info("add lidar-inertial alignment factors for '{}' and '{}', align step: {}", - lidarTopic, Configor::DataStream::ReferIMU); + lidarTopic, Configor::DataStream::ReferIMU, ALIGN_STEP); for (int i = 0; i < static_cast(poseSeq.size()) - ALIGN_STEP; ++i) { const auto &sPose = poseSeq.at(i), ePose = poseSeq.at(i + ALIGN_STEP); @@ -254,7 +254,7 @@ void CalibSolver::InitSensorInertialAlign() const { Configor::DataStream::IMUTopics.at(topic).AcceWeight); ++count; } - spdlog::info("constraint count of inertial alignment for '{}': {}", topic, + spdlog::info("constraint count of inertial alignment for '{}'-'{}': {}", topic, Configor::DataStream::ReferIMU, count); } } From 5080722aaeb79d38a3461ee5a6cded2af39916d1 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 12:45:46 +0200 Subject: [PATCH 03/12] Add option to optimize IMU nonlinearity. The optimization of nonlinearities will only be enabled in the last batch opt. --- include/config/configor.h | 2 ++ include/solver/batch_opt_option.hpp | 7 +++++++ src/config/configor.cpp | 1 + 3 files changed, 10 insertions(+) diff --git a/include/config/configor.h b/include/config/configor.h index 70d5ae4..57ae367 100644 --- a/include/config/configor.h +++ b/include/config/configor.h @@ -257,6 +257,7 @@ struct Configor { static double GravityNorm; static constexpr int SplineOrder = 4; static bool OptTemporalParams; + static bool OptImuNonlinearity; static double TimeOffsetPadding; static double ReadoutTimePadding; static double MapDownSample; @@ -319,6 +320,7 @@ struct Configor { ar(CEREAL_NVP(SpatTempPrioriPath), CEREAL_NVP(GravityNorm), CEREAL_NVP(OptTemporalParams), CEREAL_NVP(TimeOffsetPadding), CEREAL_NVP(ReadoutTimePadding), CEREAL_NVP(MapDownSample), + CEREAL_NVP(OptImuNonlinearity), cereal::make_nvp("KnotTimeDist", knotTimeDist), cereal::make_nvp("NDTLiDAROdometer", ndtLiDAROdometer), cereal::make_nvp("LiDARDataAssociate", lidarDataAssociate)); diff --git a/include/solver/batch_opt_option.hpp b/include/solver/batch_opt_option.hpp index 6240cf3..3590b4e 100644 --- a/include/solver/batch_opt_option.hpp +++ b/include/solver/batch_opt_option.hpp @@ -137,6 +137,13 @@ struct BatchOptOption { if (Configor::IsEventIntegrated()) { options = MergeOptions(options, AryToVecWithAppend(MultiEventIMU)); } + // Optimize IMU nonlinearity in the last batch + if (!options.empty() && Configor::Prior::OptImuNonlinearity) { + auto& opt = options.back(); + opt |= Opt::OPT_GYRO_MAP_COEFF; + opt |= Opt::OPT_ACCE_MAP_COEFF; + opt |= Opt::OPT_SO3_AtoG; + } } if (options.empty()) { throw Status(Status::CRITICAL, "unknown error happened! (unknown sensor suite)"); diff --git a/src/config/configor.cpp b/src/config/configor.cpp index f213b7c..a6e91f7 100644 --- a/src/config/configor.cpp +++ b/src/config/configor.cpp @@ -121,6 +121,7 @@ const double Configor::Prior::LossForReprojFactor = 1.0; const double Configor::Prior::LossForOpticalFlowFactor = 30.0; bool Configor::Prior::OptTemporalParams = {}; +bool Configor::Prior::OptImuNonlinearity = false; bool Configor::Preference::UseCudaInSolving = {}; OutputOption Configor::Preference::Outputs = OutputOption::NONE; From e99985c200935b27745f3d14ef284cdb538b41d1 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 12:53:39 +0200 Subject: [PATCH 04/12] Added OptImuNonlinearity to configs and docs. --- config/ikalibr-config.yaml | 2 ++ data/li-calib/Court-01/config-real.yaml | 2 ++ data/li-calib/Court-02/config-real.yaml | 2 ++ data/li-calib/Court-03/config-real.yaml | 2 ++ data/li-calib/Court-04/config-real.yaml | 2 ++ data/li-calib/Court-05/config-real.yaml | 2 ++ data/li-calib/Garage-01/config-real.yaml | 2 ++ data/li-calib/Garage-02/config-real.yaml | 2 ++ data/li-calib/Garage-03/config-real.yaml | 2 ++ data/li-calib/Garage-04/config-real.yaml | 2 ++ data/li-calib/Garage-05/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-14-52-11/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-14-55-10/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-15-22-04/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-15-30-05/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-16-34-01/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-16-42-32/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-16-44-05/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-16-49-39/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-16-55-29/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-17-10-20/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-17-12-05/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-17-17-17/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-17-33-29/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-17-37-20/config-real.yaml | 2 ++ data/river/data_202392614595/config-real.yaml | 2 ++ data/river/data_202392615561/config-real.yaml | 2 ++ data/river/data_202392616822/config-real.yaml | 2 ++ data/tum/rs-seq-1/config-real.yaml | 2 ++ data/tum/rs-seq-10/config-real.yaml | 2 ++ data/tum/rs-seq-2/config-real.yaml | 2 ++ data/tum/rs-seq-3/config-real.yaml | 2 ++ data/tum/rs-seq-4/config-real.yaml | 2 ++ data/tum/rs-seq-5/config-real.yaml | 2 ++ data/tum/rs-seq-6/config-real.yaml | 2 ++ data/tum/rs-seq-7/config-real.yaml | 2 ++ data/tum/rs-seq-8/config-real.yaml | 2 ++ data/tum/rs-seq-9/config-real.yaml | 2 ++ data/vector/desk-fast/config-real.yaml | 2 ++ data/vector/hdr-fast/config-real.yaml | 2 ++ data/vector/mountain-fast/config-real.yaml | 2 ++ data/vector/robot-fast/config-real.yaml | 2 ++ data/vector/sofa-fast/config-real.yaml | 2 ++ docs/details/config_template_note.md | 2 ++ 44 files changed, 88 insertions(+) diff --git a/config/ikalibr-config.yaml b/config/ikalibr-config.yaml index 4c047ea..abd686e 100644 --- a/config/ikalibr-config.yaml +++ b/config/ikalibr-config.yaml @@ -144,6 +144,8 @@ Configor: # if sensor are hardware-synchronized, you could choose to fix temporal parameters # by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-01/config-real.yaml b/data/li-calib/Court-01/config-real.yaml index bb4bb22..7c896fe 100644 --- a/data/li-calib/Court-01/config-real.yaml +++ b/data/li-calib/Court-01/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-02/config-real.yaml b/data/li-calib/Court-02/config-real.yaml index 413fbc2..ac2aff3 100644 --- a/data/li-calib/Court-02/config-real.yaml +++ b/data/li-calib/Court-02/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-03/config-real.yaml b/data/li-calib/Court-03/config-real.yaml index cf74a3d..5ebebb1 100644 --- a/data/li-calib/Court-03/config-real.yaml +++ b/data/li-calib/Court-03/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-04/config-real.yaml b/data/li-calib/Court-04/config-real.yaml index ca4fc8f..9fafbf1 100644 --- a/data/li-calib/Court-04/config-real.yaml +++ b/data/li-calib/Court-04/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-05/config-real.yaml b/data/li-calib/Court-05/config-real.yaml index 001ae9e..f1d7c87 100644 --- a/data/li-calib/Court-05/config-real.yaml +++ b/data/li-calib/Court-05/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-01/config-real.yaml b/data/li-calib/Garage-01/config-real.yaml index aa012b0..8ba896a 100644 --- a/data/li-calib/Garage-01/config-real.yaml +++ b/data/li-calib/Garage-01/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-02/config-real.yaml b/data/li-calib/Garage-02/config-real.yaml index 3ad38cd..3e29fc4 100644 --- a/data/li-calib/Garage-02/config-real.yaml +++ b/data/li-calib/Garage-02/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-03/config-real.yaml b/data/li-calib/Garage-03/config-real.yaml index eac534b..918aed7 100644 --- a/data/li-calib/Garage-03/config-real.yaml +++ b/data/li-calib/Garage-03/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-04/config-real.yaml b/data/li-calib/Garage-04/config-real.yaml index 7b6e75d..5dfe8fb 100644 --- a/data/li-calib/Garage-04/config-real.yaml +++ b/data/li-calib/Garage-04/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-05/config-real.yaml b/data/li-calib/Garage-05/config-real.yaml index 6c575fd..675b514 100644 --- a/data/li-calib/Garage-05/config-real.yaml +++ b/data/li-calib/Garage-05/config-real.yaml @@ -67,6 +67,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml index 8945b39..9894401 100644 --- a/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml @@ -84,6 +84,8 @@ Configor: SpatTempPrioriPath: "/home/csl/ros_ws/iKalibr/src/ikalibr/config/spat-temp-priori.yaml" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml index 02cf043..c5de1f7 100644 --- a/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml @@ -84,6 +84,8 @@ Configor: SpatTempPrioriPath: "/home/csl/ros_ws/iKalibr/src/ikalibr/config/spat-temp-priori.yaml" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml index e8ae3d8..9cbbfa8 100644 --- a/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml @@ -68,6 +68,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml index 9ed7489..ca5212d 100644 --- a/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml @@ -68,6 +68,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml index 3cc20fd..c7e35ec 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml @@ -68,6 +68,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml index 78595d2..51526a7 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml @@ -86,6 +86,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml index 12c4219..7e41dd8 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml @@ -86,6 +86,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml index d6652b3..4e9c557 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml index 2bda1d8..e68c9b3 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml @@ -82,6 +82,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml index 1bf89da..f479105 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml @@ -84,6 +84,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml index 3e80952..9eca647 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml @@ -84,6 +84,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml index 9765ccd..645aa60 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml @@ -68,6 +68,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml index 84ecbfd..8889505 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml @@ -68,6 +68,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml index da34b0f..108611d 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml @@ -80,6 +80,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392614595/config-real.yaml b/data/river/data_202392614595/config-real.yaml index d5b1a3a..021af87 100644 --- a/data/river/data_202392614595/config-real.yaml +++ b/data/river/data_202392614595/config-real.yaml @@ -59,6 +59,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392615561/config-real.yaml b/data/river/data_202392615561/config-real.yaml index ed2e101..c30c670 100644 --- a/data/river/data_202392615561/config-real.yaml +++ b/data/river/data_202392615561/config-real.yaml @@ -59,6 +59,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392616822/config-real.yaml b/data/river/data_202392616822/config-real.yaml index 8bf2dae..6418745 100644 --- a/data/river/data_202392616822/config-real.yaml +++ b/data/river/data_202392616822/config-real.yaml @@ -59,6 +59,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-1/config-real.yaml b/data/tum/rs-seq-1/config-real.yaml index f712c7d..08b44f7 100644 --- a/data/tum/rs-seq-1/config-real.yaml +++ b/data/tum/rs-seq-1/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-10/config-real.yaml b/data/tum/rs-seq-10/config-real.yaml index 8068855..f118154 100644 --- a/data/tum/rs-seq-10/config-real.yaml +++ b/data/tum/rs-seq-10/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-2/config-real.yaml b/data/tum/rs-seq-2/config-real.yaml index 85a6914..85c65aa 100644 --- a/data/tum/rs-seq-2/config-real.yaml +++ b/data/tum/rs-seq-2/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-3/config-real.yaml b/data/tum/rs-seq-3/config-real.yaml index 96d2f18..912b779 100644 --- a/data/tum/rs-seq-3/config-real.yaml +++ b/data/tum/rs-seq-3/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-4/config-real.yaml b/data/tum/rs-seq-4/config-real.yaml index 8214144..78f5661 100644 --- a/data/tum/rs-seq-4/config-real.yaml +++ b/data/tum/rs-seq-4/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-5/config-real.yaml b/data/tum/rs-seq-5/config-real.yaml index 156cedc..5f0e78c 100644 --- a/data/tum/rs-seq-5/config-real.yaml +++ b/data/tum/rs-seq-5/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-6/config-real.yaml b/data/tum/rs-seq-6/config-real.yaml index 15fd205..e394e70 100644 --- a/data/tum/rs-seq-6/config-real.yaml +++ b/data/tum/rs-seq-6/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-7/config-real.yaml b/data/tum/rs-seq-7/config-real.yaml index 82a06c7..bb18e4c 100644 --- a/data/tum/rs-seq-7/config-real.yaml +++ b/data/tum/rs-seq-7/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-8/config-real.yaml b/data/tum/rs-seq-8/config-real.yaml index 45f9c0f..6d49a36 100644 --- a/data/tum/rs-seq-8/config-real.yaml +++ b/data/tum/rs-seq-8/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-9/config-real.yaml b/data/tum/rs-seq-9/config-real.yaml index 2744250..edeb126 100644 --- a/data/tum/rs-seq-9/config-real.yaml +++ b/data/tum/rs-seq-9/config-real.yaml @@ -63,6 +63,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/desk-fast/config-real.yaml b/data/vector/desk-fast/config-real.yaml index 164e60d..718a0e5 100644 --- a/data/vector/desk-fast/config-real.yaml +++ b/data/vector/desk-fast/config-real.yaml @@ -95,6 +95,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/hdr-fast/config-real.yaml b/data/vector/hdr-fast/config-real.yaml index 152465a..9b75938 100644 --- a/data/vector/hdr-fast/config-real.yaml +++ b/data/vector/hdr-fast/config-real.yaml @@ -90,6 +90,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/mountain-fast/config-real.yaml b/data/vector/mountain-fast/config-real.yaml index 4e68c88..49ac923 100644 --- a/data/vector/mountain-fast/config-real.yaml +++ b/data/vector/mountain-fast/config-real.yaml @@ -90,6 +90,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/robot-fast/config-real.yaml b/data/vector/robot-fast/config-real.yaml index d501fe6..5926a8a 100644 --- a/data/vector/robot-fast/config-real.yaml +++ b/data/vector/robot-fast/config-real.yaml @@ -90,6 +90,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/sofa-fast/config-real.yaml b/data/vector/sofa-fast/config-real.yaml index 3a04376..3696f02 100644 --- a/data/vector/sofa-fast/config-real.yaml +++ b/data/vector/sofa-fast/config-real.yaml @@ -90,6 +90,8 @@ Configor: SpatTempPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/docs/details/config_template_note.md b/docs/details/config_template_note.md index 116ebf7..a5cb583 100644 --- a/docs/details/config_template_note.md +++ b/docs/details/config_template_note.md @@ -165,6 +165,8 @@ Configor: # if sensor are hardware-synchronized, you could choose to fix temporal parameters # by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time From 11a66d8815e6474836854c430e43d44deee8aa2b Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:14:40 +0200 Subject: [PATCH 05/12] Do not optimize reference IMU extrinsics when given as Sen1 in priors. --- src/calib/spat_temp_priori.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/calib/spat_temp_priori.cpp b/src/calib/spat_temp_priori.cpp index 11853f2..6ae7b75 100644 --- a/src/calib/spat_temp_priori.cpp +++ b/src/calib/spat_temp_priori.cpp @@ -255,6 +255,10 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, // only one of the param block has been added to problem, we then add the constraint, // to make sure a unique least-squares solution estimator.AddPriorExtriSO3Constraint(Sen1ToSen2, rot1, rot2, PrioriWeight); + if (sen1 == RefIMU && estimator.HasParameterBlock(rot1->data())) { + *rot1 = Sophus::SO3d(); + estimator.SetParameterBlockConstant(rot1->data()); + } } } for (const auto& [sensorPair, Sen1InSen2] : this->POS_Sen1InSen2) { @@ -270,6 +274,10 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, } else if (estimator.HasParameterBlock(pos1->data()) || estimator.HasParameterBlock(pos2->data())) { estimator.AddPriorExtriPOSConstraint(Sen1InSen2, pos1, rot2, pos2, PrioriWeight); + if (sen1 == RefIMU && estimator.HasParameterBlock(pos1->data())) { + *pos1 = Eigen::Vector3d::Zero(); + estimator.SetParameterBlockConstant(pos1->data()); + } } } for (const auto& [sensorPair, Sen1ToSen2] : this->TO_Sen1ToSen2) { @@ -283,6 +291,10 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, } } else if (estimator.HasParameterBlock(to1) || estimator.HasParameterBlock(to2)) { estimator.AddPriorTimeOffsetConstraint(Sen1ToSen2, to1, to2, PrioriWeight); + if (sen1 == RefIMU && estimator.HasParameterBlock(to1)) { + *to1 = 0.0; + estimator.SetParameterBlockConstant(to1); + } } } // readout times (we set them as constraints in optimization) From 29d664b33e8c0bab05f4de60f9b288be8891ba1e Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:26:06 +0200 Subject: [PATCH 06/12] Initialize extrinsics from priors if given. If the priors of all extrinsics and time offset are given for a sensor, there is no need to initialize the extrinsics and time offset using visual odometry or other approaches. If you want the priors to be non-absolute (i.e. still allow their optimization), set them as RefImu-Sen2 or Sen1-Sen2 priors. If the priors should be absolute (non-optimizable), set them as Sen1-RefImu. --- include/calib/spat_temp_priori.h | 17 ++++ src/calib/spat_temp_priori.cpp | 90 +++++++++++++++++++ .../calib_solver_init_prep_ii_align_impl.cpp | 16 +++- .../calib_solver_init_prep_li_align_impl.cpp | 21 +++++ .../calib_solver_init_prep_pci_align_impl.cpp | 42 +++++++-- ...ib_solver_init_sen_inertial_align_impl.cpp | 84 ++++++++++++++--- 6 files changed, 249 insertions(+), 21 deletions(-) diff --git a/include/calib/spat_temp_priori.h b/include/calib/spat_temp_priori.h index 769fb85..001f59a 100644 --- a/include/calib/spat_temp_priori.h +++ b/include/calib/spat_temp_priori.h @@ -76,6 +76,18 @@ class SpatialTemporalPriori { [[nodiscard]] const std::map &GetReadout() const; + [[nodiscard]] bool HasSO3ToBr(const std::string& topic) const; + + [[nodiscard]] std::optional GetSO3ToBr(const std::string& topic) const; + + [[nodiscard]] bool HasPosInBr(const std::string& topic) const; + + [[nodiscard]] std::optional GetPosInBr(const std::string& topic) const; + + [[nodiscard]] bool HasTOToBr(const std::string& topic) const; + + [[nodiscard]] std::optional GetTOToBr(const std::string& topic) const; + void CheckValidityWithConfigor() const; void AddSpatTempPrioriConstraint(Estimator &estimator, CalibParamManager &parMagr) const; @@ -94,6 +106,11 @@ class SpatialTemporalPriori { return {false, {}}; } +private: + mutable std::set hasSO3ToBr; + mutable std::set hasPosToBr; + mutable std::set hasTOToBr; + public: // Serialization template diff --git a/src/calib/spat_temp_priori.cpp b/src/calib/spat_temp_priori.cpp index 6ae7b75..c28b740 100644 --- a/src/calib/spat_temp_priori.cpp +++ b/src/calib/spat_temp_priori.cpp @@ -68,6 +68,71 @@ const std::map& SpatialTemporalPriori::GetReadout() const { return RS_READOUT; } +bool SpatialTemporalPriori::HasSO3ToBr(const std::string& topic) const { + return hasSO3ToBr.find(topic) != hasSO3ToBr.end(); +} + +std::optional SpatialTemporalPriori::GetSO3ToBr(const std::string& topic) const { + if (!HasSO3ToBr(topic)) + return {}; + + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, so3] : this->SO3_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (to == refImu && from == topic) { + return so3; + } else if (from == refImu && to == topic) { + return so3.inverse(); + } + } + return {}; +} + +bool SpatialTemporalPriori::HasPosInBr(const std::string& topic) const { + return hasPosToBr.find(topic) != hasPosToBr.end(); +} + +std::optional SpatialTemporalPriori::GetPosInBr(const std::string& topic) const { + if (!HasPosInBr(topic)) + return {}; + + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, pos] : this->POS_Sen1InSen2) { + const auto& [from, to] = fromTo; + if (to == refImu && from == topic) { + return pos; + } else if (from == refImu && to == topic) { + // we know the so3 exists because of the HasPosToBr() check above + const auto so3 = GetSO3ToBr(to); + return (*so3) * (-pos); + } + } + return {}; +} + +bool SpatialTemporalPriori::HasTOToBr(const std::string& topic) const { + return hasTOToBr.find(topic) != hasTOToBr.end(); +} + +std::optional SpatialTemporalPriori::GetTOToBr(const std::string& topic) const { + if (!HasTOToBr(topic)) + return {}; + + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, offset] : this->TO_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (to == refImu && from == topic) { + return offset; + } else if (from == refImu && to == topic) { + return -offset; + } + } + return {}; +} + void SpatialTemporalPriori::CheckValidityWithConfigor() const { // check map if its ambiguous if (auto [res, p] = IsMapAmbiguous(this->SO3_Sen1ToSen2); res) { @@ -173,6 +238,31 @@ void SpatialTemporalPriori::CheckValidityWithConfigor() const { readout, sensor, RT_PADDING); } } + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, _] : SO3_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (from == refImu) + hasSO3ToBr.insert(to); + else if (to == refImu) + hasSO3ToBr.insert(from); + } + + for (const auto& [fromTo, _] : POS_Sen1InSen2) { + const auto& [from, to] = fromTo; + if (from == refImu) + hasPosToBr.insert(to); + else if (to == refImu && hasSO3ToBr.find(from) != hasSO3ToBr.end()) + hasPosToBr.insert(from); + } + + for (const auto& [fromTo, _] : TO_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (from == refImu) + hasTOToBr.insert(to); + else if (to == refImu) + hasTOToBr.insert(from); + } } void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, diff --git a/src/solver/calib_solver_init_prep_ii_align_impl.cpp b/src/solver/calib_solver_init_prep_ii_align_impl.cpp index 4ee2b54..4e7ee5b 100644 --- a/src/solver/calib_solver_init_prep_ii_align_impl.cpp +++ b/src/solver/calib_solver_init_prep_ii_align_impl.cpp @@ -32,6 +32,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include "calib/calib_param_manager.h" +#include "calib/spat_temp_priori.h" #include "solver/calib_solver.h" namespace { @@ -41,7 +43,19 @@ bool IKALIBR_UNIQUE_NAME(_2_) = ns_ikalibr::_1_(__FILE__); namespace ns_ikalibr { void CalibSolver::InitPrepInertialInertialAlign() { - // There is no need to prepare for inertial-inertial alignment + for (const auto& [topic, _] : Configor::DataStream::IMUTopics) { + if (const auto so3 = _priori->GetSO3ToBr(topic)) { + _parMagr->EXTRI.SO3_BiToBr.at(topic) = *so3; + spdlog::info("extrinsic rotation read from priori information for IMU '{}'", topic); + } + } + + for (const auto& [topic, _] : Configor::DataStream::IMUTopics) { + if (const auto offset = _priori->GetTOToBr(topic)) { + _parMagr->TEMPORAL.TO_BiToBr.at(topic) = *offset; + spdlog::info("time offset read from priori information for IMU '{}'", topic); + } + } } } // namespace ns_ikalibr \ No newline at end of file diff --git a/src/solver/calib_solver_init_prep_li_align_impl.cpp b/src/solver/calib_solver_init_prep_li_align_impl.cpp index f06b32b..baba6d1 100644 --- a/src/solver/calib_solver_init_prep_li_align_impl.cpp +++ b/src/solver/calib_solver_init_prep_li_align_impl.cpp @@ -35,6 +35,7 @@ #include "calib/calib_data_manager.h" #include "calib/calib_param_manager.h" #include "calib/estimator.h" +#include "calib/spat_temp_priori.h" #include "core/lidar_odometer.h" #include "core/rotation_estimator.h" #include "core/scan_undistortion.h" @@ -55,6 +56,21 @@ void CalibSolver::InitPrepLiDARInertialAlign() const { } const auto &so3Spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); const auto &scaleSpline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); + + for (const auto& [topic, _] : Configor::DataStream::LiDARTopics) { + if (const auto so3 = _priori->GetSO3ToBr(topic)) { + _parMagr->EXTRI.SO3_LkToBr.at(topic) = *so3; + spdlog::info("extrinsic rotation read from priori information for lidar '{}'", topic); + } + } + + for (const auto& [topic, _] : Configor::DataStream::LiDARTopics) { + if (const auto offset = _priori->GetTOToBr(topic)) { + _parMagr->TEMPORAL.TO_LkToBr.at(topic) = *offset; + spdlog::info("time offset read from priori information for lidar '{}'", topic); + } + } + /** * we throw the head and tail data as the rotations from the fitted SO3 Spline in that range are * poor @@ -70,6 +86,11 @@ void CalibSolver::InitPrepLiDARInertialAlign() const { */ spdlog::info("LiDARs are integrated, initializing extrinsic rotations of LiDARs..."); for (const auto &[topic, data] : _dataMagr->GetLiDARMeasurements()) { + const auto hasSO3 = _priori->HasSO3ToBr(topic); + const auto hasTO = _priori->HasTOToBr(topic); + if (hasSO3 && (!Configor::Prior::OptTemporalParams || hasTO)) + continue; + spdlog::info("performing ndt odometer for '{}' for extrinsic rotation initialization...", topic); diff --git a/src/solver/calib_solver_init_prep_pci_align_impl.cpp b/src/solver/calib_solver_init_prep_pci_align_impl.cpp index 2cb18ea..432c17b 100644 --- a/src/solver/calib_solver_init_prep_pci_align_impl.cpp +++ b/src/solver/calib_solver_init_prep_pci_align_impl.cpp @@ -35,6 +35,7 @@ #include "calib/calib_data_manager.h" #include "calib/calib_param_manager.h" #include "calib/estimator.h" +#include "calib/spat_temp_priori.h" #include "core/rotation_estimator.h" #include "core/vision_only_sfm.h" #include "opencv2/highgui.hpp" @@ -53,6 +54,21 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { } const auto& so3Spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); const auto& scaleSpline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); + + for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + if (const auto so3 = _priori->GetSO3ToBr(topic)) { + _parMagr->EXTRI.SO3_CmToBr.at(topic) = *so3; + spdlog::info("extrinsic rotation read from priori information for camera '{}'", topic); + } + } + + for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + if (const auto offset = _priori->GetTOToBr(topic)) { + _parMagr->TEMPORAL.TO_CmToBr.at(topic) = *offset; + spdlog::info("time offset read from priori information for camera '{}'", topic); + } + } + /** * we throw the head and tail data as the rotations from the fitted SO3 Spline in that range are * poor @@ -74,6 +90,11 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { // the min distance between two features (to ensure features are distributed uniformly) constexpr int minDist = 25; for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + const auto hasSO3 = _priori->HasSO3ToBr(topic); + const auto hasTO = _priori->HasTOToBr(topic); + if (hasSO3 && (!Configor::Prior::OptTemporalParams || hasTO)) + continue; + const auto& frameVec = _dataMagr->GetCameraMeasurements(topic); spdlog::info( "perform rotation-only visual odometer to recover extrinsic rotations for '{}'...", @@ -96,6 +117,8 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { spdlog::warn( "tracking failed when grab the '{}' image frame!!! try to reinitialize", i); } + if (hasSO3) + continue; // we do not want to try to recover the extrinsic rotation too frequent if ((odometer->GetRotations().size() < 50) || @@ -115,13 +138,15 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { break; } } - if (!rotEstimator->SolveStatus()) { - throw Status(Status::ERROR, - "initialize rotation 'SO3_CmToBr' failed, this may be related to " - "insufficiently excited motion or bad images."); - } else { - spdlog::info("extrinsic rotation of '{}' is recovered using '{:06}' frames", topic, - odometer->GetRotations().size()); + if (!hasSO3) { + if (!rotEstimator->SolveStatus()) { + throw Status(Status::ERROR, + "initialize rotation 'SO3_CmToBr' failed, this may be related to " + "insufficiently excited motion or bad images."); + } else { + spdlog::info("extrinsic rotation of '{}' is recovered using '{:06}' frames", topic, + odometer->GetRotations().size()); + } } _viewer->UpdateSensorViewer(); @@ -141,6 +166,9 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { // perform time offset estimation and extrinsic rotation refinement for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + if (_priori->HasTOToBr(topic)) + continue; + const auto& rotations = rotOnlyOdom.at(topic)->GetRotations(); // this field should be zero here double TO_CmToBr = _parMagr->TEMPORAL.TO_CmToBr.at(topic); diff --git a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp index 2361176..31e512a 100644 --- a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp +++ b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp @@ -35,6 +35,7 @@ #include "calib/calib_data_manager.h" #include "calib/calib_param_manager.h" #include "calib/estimator.h" +#include "calib/spat_temp_priori.h" #include "core/lidar_odometer.h" #include "solver/calib_solver.h" #include "util/utils_tpl.hpp" @@ -102,6 +103,14 @@ void CalibSolver::InitSensorInertialAlign() const { const auto &poseSeq = odometer->GetOdomPoseVec(); double TO_LkToBr = _parMagr->TEMPORAL.TO_LkToBr.at(lidarTopic); + auto lidarOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(lidarTopic)) { + _parMagr->EXTRI.POS_LkInBr.at(lidarTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for lidar '{}'", + lidarTopic); + lidarOptOption &= ~OptOption::OPT_POS_LkInBr; + } + // create linear velocity sequence linVelSeqLk[lidarTopic] = std::vector(poseSeq.size(), Eigen::Vector3d::Zero()); @@ -136,7 +145,7 @@ void CalibSolver::InitSensorInertialAlign() const { odometer->GetMapTime(), // the map time &curLidarLinVelSeq.at(i), // the start velocity (to be estimated) &curLidarLinVelSeq.at(i + ALIGN_STEP), // the end velocity (to be estimated) - optOption, // the optimize option + lidarOptOption, // the optimize option weight); // the weigh } } @@ -185,6 +194,14 @@ void CalibSolver::InitSensorInertialAlign() const { frame->GetTimestamp()); } + auto camOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(camTopic)) { + _parMagr->EXTRI.POS_CmInBr.at(camTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for camera '{}'", + camTopic); + camOptOption &= ~OptOption::OPT_POS_CmInBr; + } + // create linear velocity sequence linVelSeqCm[camTopic] = std::vector(constructedFrames.size(), Eigen::Vector3d::Zero()); @@ -226,7 +243,7 @@ void CalibSolver::InitSensorInertialAlign() const { &curCamLinVelSeq.at(i), // the start velocity (to be estimated) &curCamLinVelSeq.at(i + ALIGN_STEP), // the end velocity (to be estimated) &scale, // the visual scale (to be estimated) - optOption, // the optimize option + camOptOption, // the optimize option weight); // the weigh } } @@ -236,6 +253,15 @@ void CalibSolver::InitSensorInertialAlign() const { std::vector linVelSeqBr(std::floor((et - st) / dt), Eigen::Vector3d::Zero()); if (Configor::DataStream::IMUTopics.size() >= 2) { for (const auto &[topic, frames] : _dataMagr->GetIMUMeasurements()) { + auto imuOptOption = optOption; + const auto pos = _priori->GetPosInBr(topic); + if (topic != Configor::DataStream::ReferIMU && pos) { + _parMagr->EXTRI.POS_BiInBr.at(topic) = *pos; + spdlog::info("extrinsic translation read from priori information for IMU '{}'", + topic); + imuOptOption &= ~OptOption::OPT_POS_BiInBr; + } + spdlog::info("add inertial alignment factors for '{}'...", topic); int count = 0; for (int i = 0; i < static_cast(linVelSeqBr.size()) - 1; ++i) { @@ -244,13 +270,13 @@ void CalibSolver::InitSensorInertialAlign() const { Eigen::Vector3d *sVel = &linVelSeqBr.at(sIdx), *eVel = &linVelSeqBr.at(eIdx); estimator->AddInertialAlignment( - frames, // imu frames - topic, // the ros topic of this imu - sTimeByBr, // the start time stamped by the reference imu - eTimeByBr, // the end time stamped by the reference imu - sVel, // the start velocity (to be estimated) - eVel, // the end velocity (to be estimated) - optOption, // the optimize option + frames, // imu frames + topic, // the ros topic of this imu + sTimeByBr, // the start time stamped by the reference imu + eTimeByBr, // the end time stamped by the reference imu + sVel, // the start velocity (to be estimated) + eVel, // the end velocity (to be estimated) + imuOptOption, // the optimize option Configor::DataStream::IMUTopics.at(topic).AcceWeight); ++count; } @@ -266,6 +292,14 @@ void CalibSolver::InitSensorInertialAlign() const { const auto &refIMUFrames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto radarOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(radarTopic)) { + _parMagr->EXTRI.POS_RjInBr.at(radarTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for radar '{}'", + radarTopic); + radarOptOption &= ~OptOption::OPT_POS_RjInBr; + } + const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetRadarAvgFrequency(radarTopic))); @@ -308,7 +342,7 @@ void CalibSolver::InitSensorInertialAlign() const { radarTopic, // the ros topic of this radar sArray, // the start target array eArray, // the end target array - optOption, // the optimization option + radarOptOption, // the optimization option weight); // the weight ++count; } @@ -323,6 +357,14 @@ void CalibSolver::InitSensorInertialAlign() const { const auto &frames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto rgbdOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(rgbdTopic)) { + _parMagr->EXTRI.POS_DnInBr.at(rgbdTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for rgbd camera '{}'", + rgbdTopic); + rgbdOptOption &= ~OptOption::OPT_POS_DnInBr; + } + const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetRGBDAvgFrequency(rgbdTopic))); @@ -351,7 +393,7 @@ void CalibSolver::InitSensorInertialAlign() const { rgbdTopic, // the ros topic of this rgbd camera bodyFrameVels.at(i), // the start velocity bodyFrameVels.at(i + ALIGN_STEP), // the end velocity - optOption, // the optimization option + rgbdOptOption, // the optimization option weight); // the weight ++count; } @@ -366,6 +408,14 @@ void CalibSolver::InitSensorInertialAlign() const { double TO_CmToBr = _parMagr->TEMPORAL.TO_CmToBr.at(topic); const auto &frames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto camOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(topic)) { + _parMagr->EXTRI.POS_CmInBr.at(topic) = *pos; + spdlog::info("extrinsic translation read from priori information for camera '{}'", + topic); + camOptOption &= ~OptOption::OPT_POS_CmInBr; + } + const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetCameraAvgFrequency(topic))); @@ -401,7 +451,7 @@ void CalibSolver::InitSensorInertialAlign() const { &curVelScales.at(i), // the scale of start velocity to be estimated velDirs.at(i + ALIGN_STEP), // the direction of the end velocity &curVelScales.at(i + ALIGN_STEP), // the scale of end velocity to be estimated - optOption, // the optimization option + camOptOption, // the optimization option weight); // the weight ++count; } @@ -416,6 +466,14 @@ void CalibSolver::InitSensorInertialAlign() const { double TO_EsToBr = _parMagr->TEMPORAL.TO_EsToBr.at(topic); const auto &frames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto eventOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(topic)) { + _parMagr->EXTRI.POS_EsInBr.at(topic) = *pos; + spdlog::info("extrinsic translation read from priori information for event camera '{}'", + topic); + eventOptOption &= ~OptOption::OPT_POS_EsInBr; + } + double freq = static_cast(velDirs.size()) / (velDirs.back().first - velDirs.front().first); const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * freq)); @@ -450,7 +508,7 @@ void CalibSolver::InitSensorInertialAlign() const { &curVelScales.at(i), // the scale of start velocity to be estimated velDirs.at(i + ALIGN_STEP), // the direction of the end velocity &curVelScales.at(i + ALIGN_STEP), // the scale of end velocity to be estimated - optOption, // the optimization option + eventOptOption, // the optimization option weight); // the weight ++count; } From e14074604f685bfa26d05850337ef7bea1746cc2 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:30:48 +0200 Subject: [PATCH 07/12] Allow setting priors on gravity direction and minimum visual scale. This speeds up optimization and prevents wrongly estimated visual scale if you know the optimization sometimes tend to squeeze the camera using 0.001 scale. --- include/calib/spat_temp_priori.h | 8 ++++- src/calib/spat_temp_priori.cpp | 35 +++++++++++++++++++ ...ib_solver_init_sen_inertial_align_impl.cpp | 9 +++++ 3 files changed, 51 insertions(+), 1 deletion(-) diff --git a/include/calib/spat_temp_priori.h b/include/calib/spat_temp_priori.h index 001f59a..26a503a 100644 --- a/include/calib/spat_temp_priori.h +++ b/include/calib/spat_temp_priori.h @@ -62,6 +62,8 @@ class SpatialTemporalPriori { std::map TO_Sen1ToSen2; // readout time of rs cameras std::map RS_READOUT; + Eigen::Vector3d GRAVITY; + std::map MIN_VISUAL_SCALE; public: SpatialTemporalPriori() = default; @@ -76,6 +78,10 @@ class SpatialTemporalPriori { [[nodiscard]] const std::map &GetReadout() const; + [[nodiscard]] std::optional GetGravity() const; + + [[nodiscard]] const std::map &GetMinVisualScale() const; + [[nodiscard]] bool HasSO3ToBr(const std::string& topic) const; [[nodiscard]] std::optional GetSO3ToBr(const std::string& topic) const; @@ -116,7 +122,7 @@ class SpatialTemporalPriori { template void serialize(Archive &ar) { ar(CEREAL_NVP(SO3_Sen1ToSen2), CEREAL_NVP(POS_Sen1InSen2), CEREAL_NVP(TO_Sen1ToSen2), - CEREAL_NVP(RS_READOUT)); + CEREAL_NVP(RS_READOUT), CEREAL_NVP(GRAVITY), CEREAL_NVP(MIN_VISUAL_SCALE)); } // save the parameters to file using cereal library diff --git a/src/calib/spat_temp_priori.cpp b/src/calib/spat_temp_priori.cpp index c28b740..6ae30dd 100644 --- a/src/calib/spat_temp_priori.cpp +++ b/src/calib/spat_temp_priori.cpp @@ -68,6 +68,16 @@ const std::map& SpatialTemporalPriori::GetReadout() const { return RS_READOUT; } +std::optional SpatialTemporalPriori::GetGravity() const { + if (GRAVITY == Eigen::Vector3d::Zero()) + return {}; + return GRAVITY; +} + +const std::map& SpatialTemporalPriori::GetMinVisualScale() const { + return MIN_VISUAL_SCALE; +} + bool SpatialTemporalPriori::HasSO3ToBr(const std::string& topic) const { return hasSO3ToBr.find(topic) != hasSO3ToBr.end(); } @@ -238,6 +248,23 @@ void SpatialTemporalPriori::CheckValidityWithConfigor() const { readout, sensor, RT_PADDING); } } + + if (GRAVITY != Eigen::Vector3d::Zero()) { + if (std::abs(GRAVITY.norm() - Configor::Prior::GravityNorm) > 1e-3) { + throw Status(Status::ERROR, "the given prior gravity vector [{}, {}, {}] does not have " + "norm equal to Prior::GravityNorm ({})! The vector's norm is: {}.", + GRAVITY.x(), GRAVITY.y(), GRAVITY.z(), Configor::Prior::GravityNorm, + GRAVITY.norm()); + } + } + + for (const auto& [cam, _] : MIN_VISUAL_SCALE) { + if (optCamModelType.count(cam) == 0) { + throw Status(Status::ERROR, "MIN_VISUAL_SCALE defined for topic '{}' which is not an " + "optical camera topic!", cam); + } + } + const auto& refImu = Configor::DataStream::ReferIMU; for (const auto& [fromTo, _] : SO3_Sen1ToSen2) { @@ -395,6 +422,14 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, estimator.SetParameterBlockConstant(data); } } + const auto gravityPrior = this->GetGravity(); + if (gravityPrior) { + auto gravity = &parMagr.GRAVITY; + *gravity = *gravityPrior; + if (estimator.HasParameterBlock(gravity->data())) { + estimator.SetParameterBlockConstant(gravity->data()); + } + } spdlog::info("add spatial and temp priori constraint finished"); } diff --git a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp index 31e512a..b47f6cf 100644 --- a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp +++ b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp @@ -219,6 +219,11 @@ void CalibSolver::InitSensorInertialAlign() const { spdlog::info("add visual-inertial alignment factors for '{}' and '{}', align step: {}", camTopic, Configor::DataStream::ReferIMU, ALIGN_STEP); + std::optional minScale; + if (_priori->GetMinVisualScale().count(camTopic) > 0) { + minScale = _priori->GetMinVisualScale().at(camTopic); + } + for (int i = 0; i < static_cast(constructedFrames.size()) - ALIGN_STEP; ++i) { const auto &sPose = constructedFrames.at(i); const auto &ePose = constructedFrames.at(i + ALIGN_STEP); @@ -245,6 +250,10 @@ void CalibSolver::InitSensorInertialAlign() const { &scale, // the visual scale (to be estimated) camOptOption, // the optimize option weight); // the weigh + + if (minScale) { + estimator->SetParameterLowerBound(&scale, 0, *minScale); + } } } From cdf373805d6d647ef1d58f9350b90d60c1ea7544 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:36:07 +0200 Subject: [PATCH 08/12] Added config example for MIN_VISUAL_SCALE and GRAVITY. --- config/spat-temp-priori.yaml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/config/spat-temp-priori.yaml b/config/spat-temp-priori.yaml index ce72dfe..578f8dc 100644 --- a/config/spat-temp-priori.yaml +++ b/config/spat-temp-priori.yaml @@ -47,4 +47,16 @@ SpatialTemporalPriori: # value: 0.03 # readout time of another rs camera # - key: /sensor2/topic - # value: 0.02 \ No newline at end of file + # value: 0.02 + MIN_VISUAL_SCALE: + # if set for a camera sensor, this is the lower bound for the estimated visual scale. + # if not set, the minimum visual scale is 0.001 + #- key: "/sensor1/topic" + # value: 0.9 + GRAVITY: + # the direction of the gravity vector in the reference IMU when it is in its default, + # gravity-aligned position. The magnitude of the vector should correspond to the magnitude + # set in ikalibr config. This is just an initialization value and it will be further optimized. + r0c0: 0.0 + r1c0: 0.0 + r2c0: -9.8 \ No newline at end of file From f81d582ca7aaf668e4f8b6852d0f052182bf62a6 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:39:05 +0200 Subject: [PATCH 09/12] Allow specifying weight for intrinsic priors so that the optimized values are forced to be close to the priors. --- include/calib/calib_param_manager.h | 4 + include/calib/estimator.h | 11 +++ include/calib/spat_temp_priori.h | 1 + include/factor/prior_equality_factor.hpp | 103 +++++++++++++++++++++++ src/calib/calib_param_manager.cpp | 16 ++++ src/calib/estimator.cpp | 83 ++++++++++++++++++ src/calib/spat_temp_priori.cpp | 78 +++++++++++++++++ 7 files changed, 296 insertions(+) create mode 100644 include/factor/prior_equality_factor.hpp diff --git a/include/calib/calib_param_manager.h b/include/calib/calib_param_manager.h index 3249ee2..838f6a3 100644 --- a/include/calib/calib_param_manager.h +++ b/include/calib/calib_param_manager.h @@ -213,6 +213,10 @@ struct CalibParamManager { std::map Camera; std::map RGBD; + std::map PrioriIMU; + std::map PrioriCamera; + std::map PrioriRGBD; + static ns_veta::PinholeIntrinsic::Ptr LoadCameraIntri( const std::string &filename, CerealArchiveType::Enum archiveType = CerealArchiveType::Enum::YAML); diff --git a/include/calib/estimator.h b/include/calib/estimator.h index b8db93b..ab7f21a 100644 --- a/include/calib/estimator.h +++ b/include/calib/estimator.h @@ -485,6 +485,17 @@ class Estimator : public ceres::Problem { double *TO_Sen2ToRef, double weight); + void AddPriorEqualityConstraint(const double *prior, double *address, double weight); + void AddPriorEqualityConstraint(const Eigen::Vector3d &prior, + Eigen::Vector3d &address, + double weight); + void AddPriorEqualityConstraint(const Eigen::Vector6d &prior, + Eigen::Vector6d &address, + double weight); + void AddPriorEqualityConstraint(const Sophus::SO3d &prior, + Sophus::SO3d &address, + double weight); + void PrintUninvolvedKnots() const; void AddVisualVelocityDepthFactor(Eigen::Vector3d *LIN_VEL_CmToWInCm, diff --git a/include/calib/spat_temp_priori.h b/include/calib/spat_temp_priori.h index 26a503a..bdfb74a 100644 --- a/include/calib/spat_temp_priori.h +++ b/include/calib/spat_temp_priori.h @@ -64,6 +64,7 @@ class SpatialTemporalPriori { std::map RS_READOUT; Eigen::Vector3d GRAVITY; std::map MIN_VISUAL_SCALE; + std::map INTRI_WEIGHTS; public: SpatialTemporalPriori() = default; diff --git a/include/factor/prior_equality_factor.hpp b/include/factor/prior_equality_factor.hpp new file mode 100644 index 0000000..8b41c8e --- /dev/null +++ b/include/factor/prior_equality_factor.hpp @@ -0,0 +1,103 @@ +// iKalibr: Unified Targetless Spatiotemporal Calibration Framework +// Copyright 2024, the School of Geodesy and Geomatics (SGG), Wuhan University, China +// https://github.com/Unsigned-Long/iKalibr.git +// +// Author: Shuolong Chen (shlchen@whu.edu.cn) +// GitHub: https://github.com/Unsigned-Long +// ORCID: 0000-0002-5283-9057 +// +// Purpose: See .h/.hpp file. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * The names of its contributors can not be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef IKALIBR_PRIOR_EQUALITY_FACTOR_HPP +#define IKALIBR_PRIOR_EQUALITY_FACTOR_HPP + +#include "ctraj/utils/eigen_utils.hpp" +#include "ctraj/utils/sophus_utils.hpp" +#include "ceres/dynamic_autodiff_cost_function.h" +#include "util/utils.h" + +namespace { +bool IKALIBR_UNIQUE_NAME(_2_) = ns_ikalibr::_1_(__FILE__); +} + +namespace ns_ikalibr { +template +struct PriorEqualityFactor { +private: + const double* _prior; + double _weight; + +public: + explicit PriorEqualityFactor(const double *prior, double weight) + : _prior(prior), + _weight(weight) {} + + static auto Create(const double *prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<1>(prior, weight)); + } + + static auto Create(const Eigen::Vector3d &prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<3>(prior.data(), weight)); + } + + static auto Create(const Eigen::Vector6d &prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<6>(prior.data(), weight)); + } + + static auto Create(const Sophus::SO3d &prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<4>(prior.data(), weight)); + } + + static std::size_t TypeHashCode() { return typeid(PriorEqualityFactor).hash_code(); } + +public: + /** + * param blocks: + * [ Prior | ... ] + */ + template + bool operator()(T const *const *sKnots, T *sResiduals) const { + const T* const actual = sKnots[0]; + + Eigen::Map> residuals(sResiduals); + for (size_t i = 0; i < PriorSize; ++i) { + residuals(i) = T(_weight) * (_prior[i] - actual[i]); + } + + return true; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace ns_ikalibr + +#endif // IKALIBR_PRIOR_EQUALITY_FACTOR_HPP diff --git a/src/calib/calib_param_manager.cpp b/src/calib/calib_param_manager.cpp index 20cf4d8..3f43c6a 100644 --- a/src/calib/calib_param_manager.cpp +++ b/src/calib/calib_param_manager.cpp @@ -158,6 +158,22 @@ CalibParamManager::Ptr CalibParamManager::InitParamsFromConfigor() { } } + for (const auto& [topic, intri] : parMarg->INTRI.IMU) { + parMarg->INTRI.PrioriIMU[topic] = IMUIntrinsics::Create(); + *parMarg->INTRI.PrioriIMU[topic] = *parMarg->INTRI.IMU[topic]; + } + for (const auto& [topic, intri] : parMarg->INTRI.Camera) { + parMarg->INTRI.PrioriCamera[topic] = ns_veta::PinholeIntrinsic::Create(0, 0, 0, 0, 0, 0); + *parMarg->INTRI.PrioriCamera[topic] = *parMarg->INTRI.Camera[topic]; + } + for (const auto& [topic, intri] : parMarg->INTRI.RGBD) { + parMarg->INTRI.PrioriRGBD[topic] = RGBDIntrinsics::Create( + ns_veta::PinholeIntrinsic::Create(0, 0, 0, 0, 0, 0), 0, 0); + *parMarg->INTRI.PrioriRGBD[topic]->intri = *parMarg->INTRI.RGBD[topic]->intri; + parMarg->INTRI.PrioriRGBD[topic]->alpha = parMarg->INTRI.RGBD[topic]->alpha; + parMarg->INTRI.PrioriRGBD[topic]->beta = parMarg->INTRI.RGBD[topic]->beta; + } + // align to the negative 'z' axis parMarg->GRAVITY = Eigen::Vector3d(0.0, 0.0, -Configor::Prior::GravityNorm); diff --git a/src/calib/estimator.cpp b/src/calib/estimator.cpp index 8a5b85f..b349596 100644 --- a/src/calib/estimator.cpp +++ b/src/calib/estimator.cpp @@ -42,6 +42,7 @@ #include "factor/linear_knots_factor.hpp" #include "factor/prior_extri_pos_factor.hpp" #include "factor/prior_extri_so3_factor.hpp" +#include "factor/prior_equality_factor.hpp" #include "factor/prior_time_offset_factor.hpp" #include "factor/radar_inertial_align_factor.hpp" #include "factor/radar_inertial_rot_align_factor.hpp" @@ -1822,6 +1823,88 @@ void Estimator::AddPriorTimeOffsetConstraint(const double &TO_Sen1ToSen2, this->AddResidualBlock(costFunc, nullptr, paramBlockVec); } +void Estimator::AddPriorEqualityConstraint(const double *prior, double *address, double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<1>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(1); + + // set Residuals + costFunc->SetNumResiduals(1); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + +void Estimator::AddPriorEqualityConstraint(const Eigen::Vector3d &prior, + Eigen::Vector3d &address, + double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<3>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(3); + + // set Residuals + costFunc->SetNumResiduals(3); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address.data()); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + +void Estimator::AddPriorEqualityConstraint(const Eigen::Vector6d &prior, + Eigen::Vector6d &address, + double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<6>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(6); + + // set Residuals + costFunc->SetNumResiduals(6); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address.data()); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + +void Estimator::AddPriorEqualityConstraint(const Sophus::SO3d &prior, + Sophus::SO3d &address, + double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<4>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(4); + + // set Residuals + costFunc->SetNumResiduals(4); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address.data()); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + void Estimator::PrintUninvolvedKnots() const { { const auto &so3Knots = splines->GetSo3Spline(Configor::Preference::SO3_SPLINE).GetKnots(); diff --git a/src/calib/spat_temp_priori.cpp b/src/calib/spat_temp_priori.cpp index 6ae30dd..7e79855 100644 --- a/src/calib/spat_temp_priori.cpp +++ b/src/calib/spat_temp_priori.cpp @@ -265,6 +265,18 @@ void SpatialTemporalPriori::CheckValidityWithConfigor() const { } } + for (const auto& [topic, weight] : INTRI_WEIGHTS) { + if (optCamModelType.count(topic) == 0 && + Configor::DataStream::EventTopics.count(topic) == 0 && + Configor::DataStream::IMUTopics.count(topic) == 0) { + throw Status(Status::ERROR, "INTRI_WEIGHTS defined for topic '{}' which is neither IMU " + "nor camera topic!", topic); + } + if (weight < 0) { + throw Status(Status::ERROR, "INTRI_WEIGHTS defined for topic '{}' is negative!"); + } + } + const auto& refImu = Configor::DataStream::ReferIMU; for (const auto& [fromTo, _] : SO3_Sen1ToSen2) { @@ -430,6 +442,72 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, estimator.SetParameterBlockConstant(gravity->data()); } } + for (const auto& [topic, weight] : this->INTRI_WEIGHTS) { + if (parMagr.INTRI.IMU.count(topic) > 0) { + const auto& intri = parMagr.INTRI.IMU.at(topic); + const auto& prioriIntri = parMagr.INTRI.PrioriIMU.at(topic); + if (std::isinf(weight)) { + // set params constant + if (estimator.HasParameterBlock(intri->ACCE.BIAS.data())) { + estimator.SetParameterBlockConstant(intri->ACCE.BIAS.data()); + } + if (estimator.HasParameterBlock(intri->ACCE.MAP_COEFF.data())) { + estimator.SetParameterBlockConstant(intri->ACCE.MAP_COEFF.data()); + } + if (estimator.HasParameterBlock(intri->GYRO.BIAS.data())) { + estimator.SetParameterBlockConstant(intri->GYRO.BIAS.data()); + } + if (estimator.HasParameterBlock(intri->GYRO.MAP_COEFF.data())) { + estimator.SetParameterBlockConstant(intri->GYRO.MAP_COEFF.data()); + } + if (estimator.HasParameterBlock(intri->SO3_AtoG.data())) { + estimator.SetParameterBlockConstant(intri->SO3_AtoG.data()); + } + } else if (std::isfinite(weight)) { + estimator.AddPriorEqualityConstraint( + prioriIntri->ACCE.BIAS, intri->ACCE.BIAS, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->ACCE.MAP_COEFF, intri->ACCE.MAP_COEFF, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->GYRO.BIAS, intri->GYRO.BIAS, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->GYRO.MAP_COEFF, intri->GYRO.MAP_COEFF, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->SO3_AtoG, intri->SO3_AtoG, weight); + } + } + if (parMagr.INTRI.Camera.count(topic) > 0 || parMagr.INTRI.RGBD.count(topic) > 0) { + const auto isRGBD = parMagr.INTRI.RGBD.count(topic) > 0; + const auto& intri = isRGBD ? + parMagr.INTRI.RGBD.at(topic)->intri : parMagr.INTRI.Camera.at(topic); + const auto& prioriIntri = isRGBD ? + parMagr.INTRI.PrioriRGBD.at(topic)->intri : parMagr.INTRI.PrioriCamera.at(topic); + if (std::isinf(weight)) { + // set params constant + if (estimator.HasParameterBlock(intri->FXAddress())) { + estimator.SetParameterBlockConstant(intri->FXAddress()); + } + if (estimator.HasParameterBlock(intri->FYAddress())) { + estimator.SetParameterBlockConstant(intri->FYAddress()); + } + if (estimator.HasParameterBlock(intri->CXAddress())) { + estimator.SetParameterBlockConstant(intri->CXAddress()); + } + if (estimator.HasParameterBlock(intri->CYAddress())) { + estimator.SetParameterBlockConstant(intri->CYAddress()); + } + } else if (std::isfinite(weight)) { + estimator.AddPriorEqualityConstraint( + prioriIntri->FXAddress(), intri->FXAddress(), weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->FYAddress(), intri->FYAddress(), weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->CXAddress(), intri->CXAddress(), weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->CYAddress(), intri->CYAddress(), weight); + } + } + } spdlog::info("add spatial and temp priori constraint finished"); } From f8a895b25738d34337ee4d2a8e9ca5e44a9883a4 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:42:05 +0200 Subject: [PATCH 10/12] Added example config for INTRI_WEIGHTS. --- config/spat-temp-priori.yaml | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/config/spat-temp-priori.yaml b/config/spat-temp-priori.yaml index 578f8dc..653779c 100644 --- a/config/spat-temp-priori.yaml +++ b/config/spat-temp-priori.yaml @@ -59,4 +59,13 @@ SpatialTemporalPriori: # set in ikalibr config. This is just an initialization value and it will be further optimized. r0c0: 0.0 r1c0: 0.0 - r2c0: -9.8 \ No newline at end of file + r2c0: -9.8 + INTRI_WEIGHTS: + # these weights specify how much should the optimized intrinsics stick to their priors given as + # camera or IMU configs in the ikalibr config file. + # - key: "/camera1" + # value: 1000 + # - key: "/camera2" + # value: 1 + # - key: "/imu1" + # value: 10000 \ No newline at end of file From f248b039099a6c321a3030c9e911f810229e1a85 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:47:31 +0200 Subject: [PATCH 11/12] Added the option to specify priori for knots. This can be used e.g. for a multi-stage optimization if you know that adding a sensor will confuse the optimization. You can first optimize with other sensors, save the splines, and then run a second stage with the confusing sensor, fixing the splines so that they cannot be confused. --- include/calib/calib_param_manager.h | 3 ++ include/config/configor.h | 5 +- include/solver/calib_solver.h | 12 +++++ src/calib/estimator.cpp | 16 +++++- src/config/configor.cpp | 2 + src/solver/calib_solver_common_impl.cpp | 69 +++++++++++++++++++++++++ src/solver/calib_solver_io.cpp | 4 +- 7 files changed, 107 insertions(+), 4 deletions(-) diff --git a/include/calib/calib_param_manager.h b/include/calib/calib_param_manager.h index 838f6a3..a3e3fa6 100644 --- a/include/calib/calib_param_manager.h +++ b/include/calib/calib_param_manager.h @@ -249,6 +249,9 @@ struct CalibParamManager { // S2Manifold Eigen::Vector3d GRAVITY; + // true for every knot that has been loaded from priori information + std::map> splinesPrioriKnots; + public: // the constructor explicit CalibParamManager(const std::vector &imuTopics = {}, diff --git a/include/config/configor.h b/include/config/configor.h index 57ae367..c343a53 100644 --- a/include/config/configor.h +++ b/include/config/configor.h @@ -254,9 +254,11 @@ struct Configor { static struct Prior { static std::string SpatTempPrioriPath; + static std::string SplinesPrioriPath; static double GravityNorm; static constexpr int SplineOrder = 4; static bool OptTemporalParams; + static bool OptSplines; static bool OptImuNonlinearity; static double TimeOffsetPadding; static double ReadoutTimePadding; @@ -320,7 +322,8 @@ struct Configor { ar(CEREAL_NVP(SpatTempPrioriPath), CEREAL_NVP(GravityNorm), CEREAL_NVP(OptTemporalParams), CEREAL_NVP(TimeOffsetPadding), CEREAL_NVP(ReadoutTimePadding), CEREAL_NVP(MapDownSample), - CEREAL_NVP(OptImuNonlinearity), + CEREAL_NVP(OptImuNonlinearity), CEREAL_NVP(SplinesPrioriPath), + CEREAL_NVP(OptSplines), cereal::make_nvp("KnotTimeDist", knotTimeDist), cereal::make_nvp("NDTLiDAROdometer", ndtLiDAROdometer), cereal::make_nvp("LiDARDataAssociate", lidarDataAssociate)); diff --git a/include/solver/calib_solver.h b/include/solver/calib_solver.h index 7077a81..b1f2562 100644 --- a/include/solver/calib_solver.h +++ b/include/solver/calib_solver.h @@ -232,6 +232,12 @@ class CalibSolver { */ virtual ~CalibSolver(); + /** + * get the raw timestamp of the beginning of the used data + * @return the timestamp + */ + double GetRawStartTimestamp() const; + protected: /** * transform an input veta using given transformation information, if scale is provide, @@ -244,6 +250,12 @@ class CalibSolver { const ns_veta::Posed &curToNew, double scale); + /** + * Load spline bundle priori data. + * @param splinesPrioriPath path to the file saved by CalibSolverIO::SaveBSplines() + */ + void LoadSplineBundlePriori(const std::string& splinesPrioriPath); + /** * align vectors to a new coordinate frame where gravity pointing to negative z-axis. * the splines (both rotation and translation splines), as well as the gravity vector would be diff --git a/src/calib/estimator.cpp b/src/calib/estimator.cpp index b349596..43be6d5 100644 --- a/src/calib/estimator.cpp +++ b/src/calib/estimator.cpp @@ -106,6 +106,7 @@ void Estimator::AddRdKnotsData(std::vector ¶mBlockVec, const Estimator::SplineBundleType::RdSplineType &spline, const Estimator::SplineMetaType &splineMeta, bool setToConst) { + const auto& prioriKnots = parMagr->splinesPrioriKnots.at(Configor::Preference::SCALE_SPLINE); // for each segment for (const auto &seg : splineMeta.segments) { // the factor 'seg.dt * 0.5' is the treatment for numerical accuracy @@ -119,7 +120,12 @@ void Estimator::AddRdKnotsData(std::vector ¶mBlockVec, paramBlockVec.push_back(data); // set this param block to be constant - if (setToConst) { + bool setKnotToConst = setToConst; + if (!setToConst && !Configor::Prior::OptSplines) { + if (i < prioriKnots.size()) + setKnotToConst = prioriKnots.at(i); + } + if (setKnotToConst) { this->SetParameterBlockConstant(data); } } @@ -130,6 +136,7 @@ void Estimator::AddSo3KnotsData(std::vector ¶mBlockVec, const Estimator::SplineBundleType::So3SplineType &spline, const Estimator::SplineMetaType &splineMeta, bool setToConst) { + const auto& prioriKnots = parMagr->splinesPrioriKnots.at(Configor::Preference::SO3_SPLINE); // for each segment for (const auto &seg : splineMeta.segments) { // the factor 'seg.dt * 0.5' is the treatment for numerical accuracy @@ -143,7 +150,12 @@ void Estimator::AddSo3KnotsData(std::vector ¶mBlockVec, paramBlockVec.push_back(data); // set this param block to be constant - if (setToConst) { + bool setKnotToConst = setToConst; + if (!setToConst && !Configor::Prior::OptSplines) { + if (i < prioriKnots.size()) + setKnotToConst = prioriKnots.at(i); + } + if (setKnotToConst) { this->SetParameterBlockConstant(data); } } diff --git a/src/config/configor.cpp b/src/config/configor.cpp index a6e91f7..be30070 100644 --- a/src/config/configor.cpp +++ b/src/config/configor.cpp @@ -89,6 +89,7 @@ const std::string Configor::DataStream::PkgPath = ros::package::getPath("ikalibr const std::string Configor::DataStream::DebugPath = PkgPath + "/debug/"; std::string Configor::Prior::SpatTempPrioriPath = {}; +std::string Configor::Prior::SplinesPrioriPath = {}; double Configor::Prior::GravityNorm = {}; double Configor::Prior::TimeOffsetPadding = {}; double Configor::Prior::ReadoutTimePadding = {}; @@ -121,6 +122,7 @@ const double Configor::Prior::LossForReprojFactor = 1.0; const double Configor::Prior::LossForOpticalFlowFactor = 30.0; bool Configor::Prior::OptTemporalParams = {}; +bool Configor::Prior::OptSplines = true; bool Configor::Prior::OptImuNonlinearity = false; bool Configor::Preference::UseCudaInSolving = {}; diff --git a/src/solver/calib_solver_common_impl.cpp b/src/solver/calib_solver_common_impl.cpp index a1b31dd..416b1b4 100644 --- a/src/solver/calib_solver_common_impl.cpp +++ b/src/solver/calib_solver_common_impl.cpp @@ -110,6 +110,10 @@ CalibSolver::CalibSolver(CalibDataManager::Ptr calibDataManager, _splines = CreateSplineBundle( _dataMagr->GetCalibStartTimestamp(), _dataMagr->GetCalibEndTimestamp(), Configor::Prior::KnotTimeDist::SO3Spline, Configor::Prior::KnotTimeDist::ScaleSpline); + _parMagr->splinesPrioriKnots[Configor::Preference::SO3_SPLINE].resize( + _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE).GetKnots().size(), false); + _parMagr->splinesPrioriKnots[Configor::Preference::SCALE_SPLINE].resize( + _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE).GetKnots().size(), false); // create viewer _viewer = Viewer::Create(_parMagr, _splines); @@ -133,6 +137,12 @@ CalibSolver::CalibSolver(CalibDataManager::Ptr calibDataManager, spdlog::info("priori about spatial and temporal parameters are given: '{}'", Configor::Prior::SpatTempPrioriPath); } + + // spline bundle priori + if (std::filesystem::exists(Configor::Prior::SplinesPrioriPath)) { + LoadSplineBundlePriori(Configor::Prior::SplinesPrioriPath); + spdlog::info("priori about splines are given: '{}'", Configor::Prior::SplinesPrioriPath); + } } CalibSolver::Ptr CalibSolver::Create(const CalibDataManager::Ptr &calibDataManager, @@ -151,6 +161,10 @@ CalibSolver::~CalibSolver() { } } +double CalibSolver::GetRawStartTimestamp() const { + return _dataMagr->GetRawStartTimestamp(); +} + std::optional CalibSolver::CurBrToW(double timeByBr) const { if (GetScaleType() != TimeDeriv::LIN_POS_SPLINE) { throw Status(Status::CRITICAL, @@ -277,6 +291,57 @@ CalibSolver::SplineBundleType::Ptr CalibSolver::CreateSplineBundle(double st, return SplineBundleType::Create({so3SplineInfo, scaleSplineInfo}); } +void CalibSolver::LoadSplineBundlePriori(const std::string &splinesPrioriPath) { + // Load the format saved by CalibSolverIO::SaveBSplines() + auto prioriBundle = SplineBundleType::Create({}); + std::ifstream file(splinesPrioriPath); + auto ar = GetInputArchiveVariant(file, Configor::Preference::OutputDataFormat); + double rawStartTime; + SerializeByInputArchiveVariant(ar, Configor::Preference::OutputDataFormat, + cereal::make_nvp("splines", *prioriBundle), + cereal::make_nvp("start_time", rawStartTime)); + + try { + auto& spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); + const auto& prioriSpline = prioriBundle->GetSo3Spline(Configor::Preference::SO3_SPLINE); + auto& prioriKnots = _parMagr->splinesPrioriKnots.at(Configor::Preference::SO3_SPLINE); + size_t numPrioriKnots {0u}; + for (size_t i = 0; i < spline.GetKnots().size(); ++i) { + const double t = spline.MinTime() + static_cast(i) * spline.GetTimeInterval(); + const double prioriT = t + _dataMagr->GetRawStartTimestamp() - rawStartTime; + if (prioriSpline.TimeStampInRange(prioriT)) { + const auto [tmp, prioriI] = prioriSpline.ComputeTIndex(prioriT); + spline.GetKnot(i) = prioriSpline.GetKnot(prioriI); + prioriKnots[i] = true; + numPrioriKnots++; + } + } + spdlog::info("priori about SO3 spline are given: '{}' ({} knots)", + splinesPrioriPath, numPrioriKnots); + } catch (const std::out_of_range&) { + } + + try { + auto& spline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); + const auto& prioriSpline = prioriBundle->GetRdSpline(Configor::Preference::SCALE_SPLINE); + auto& prioriKnots = _parMagr->splinesPrioriKnots.at(Configor::Preference::SCALE_SPLINE); + size_t numPrioriKnots {0u}; + for (size_t i = 0; i < spline.GetKnots().size(); ++i) { + const double t = spline.MinTime() + static_cast(i) * spline.GetTimeInterval(); + const double prioriT = t + _dataMagr->GetRawStartTimestamp() - rawStartTime; + if (prioriSpline.TimeStampInRange(prioriT)) { + const auto [tmp, prioriI] = prioriSpline.ComputeTIndex(prioriT); + spline.GetKnot(i) = prioriSpline.GetKnot(prioriI); + prioriKnots[i] = true; + numPrioriKnots++; + } + } + spdlog::info("priori about scale spline are given: '{}' ({} knots)", + splinesPrioriPath, numPrioriKnots); + } catch (const std::out_of_range&) { + } +} + void CalibSolver::AlignStatesToGravity() const { auto &so3Spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); auto &scaleSpline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); @@ -503,6 +568,10 @@ ns_veta::Veta::Ptr CalibSolver::TryLoadSfMData(const std::string &topic, const auto &nameToOurIdx = info.GetImagesNameToIdx(); for (const auto &[IdFromColmap, image] : images) { + if (nameToOurIdx.find(image.name_) == nameToOurIdx.end()) { + spdlog::warn("Image {} from images.txt not found in info.yaml.", image.name_); + continue; + } const auto &viewId = nameToOurIdx.at(image.name_); const auto &poseId = viewId; diff --git a/src/solver/calib_solver_io.cpp b/src/solver/calib_solver_io.cpp index bb54b9a..b59ccf3 100644 --- a/src/solver/calib_solver_io.cpp +++ b/src/solver/calib_solver_io.cpp @@ -158,8 +158,10 @@ void CalibSolverIO::SaveBSplines(int hz) const { auto filename = saveDir + "/knots" + Configor::GetFormatExtension(); std::ofstream file(filename); auto ar = GetOutputArchiveVariant(file, Configor::Preference::OutputDataFormat); + const double st = _solver->GetRawStartTimestamp(); SerializeByOutputArchiveVariant(ar, Configor::Preference::OutputDataFormat, - cereal::make_nvp("splines", *_solver->_splines)); + cereal::make_nvp("splines", *_solver->_splines), + cereal::make_nvp("start_time", st)); } spdlog::info("saving splines finished!"); } From 2136c5854487f5182f5e09ab29fc21f1648aa830 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2025 13:53:29 +0200 Subject: [PATCH 12/12] Added examples and configs with SplinesPrioriPath and OptSplines. --- config/ikalibr-config.yaml | 6 ++++++ data/li-calib/Court-01/config-real.yaml | 6 ++++++ data/li-calib/Court-02/config-real.yaml | 6 ++++++ data/li-calib/Court-03/config-real.yaml | 6 ++++++ data/li-calib/Court-04/config-real.yaml | 6 ++++++ data/li-calib/Court-05/config-real.yaml | 6 ++++++ data/li-calib/Garage-01/config-real.yaml | 6 ++++++ data/li-calib/Garage-02/config-real.yaml | 6 ++++++ data/li-calib/Garage-03/config-real.yaml | 6 ++++++ data/li-calib/Garage-04/config-real.yaml | 6 ++++++ data/li-calib/Garage-05/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-14-52-11/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-14-55-10/config-real.yaml | 2 ++ .../iKalibr-data-2024-06-25-15-22-04/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-15-30-05/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-16-34-01/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-16-42-32/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-16-44-05/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-16-49-39/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-16-55-29/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-17-10-20/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-17-12-05/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-17-17-17/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-17-33-29/config-real.yaml | 6 ++++++ .../iKalibr-data-2024-06-25-17-37-20/config-real.yaml | 6 ++++++ data/river/data_202392614595/config-real.yaml | 6 ++++++ data/river/data_202392615561/config-real.yaml | 6 ++++++ data/river/data_202392616822/config-real.yaml | 6 ++++++ data/tum/rs-seq-1/config-real.yaml | 6 ++++++ data/tum/rs-seq-10/config-real.yaml | 6 ++++++ data/tum/rs-seq-2/config-real.yaml | 6 ++++++ data/tum/rs-seq-3/config-real.yaml | 6 ++++++ data/tum/rs-seq-4/config-real.yaml | 6 ++++++ data/tum/rs-seq-5/config-real.yaml | 6 ++++++ data/tum/rs-seq-6/config-real.yaml | 6 ++++++ data/tum/rs-seq-7/config-real.yaml | 6 ++++++ data/tum/rs-seq-8/config-real.yaml | 6 ++++++ data/tum/rs-seq-9/config-real.yaml | 6 ++++++ data/vector/desk-fast/config-real.yaml | 6 ++++++ data/vector/hdr-fast/config-real.yaml | 6 ++++++ data/vector/mountain-fast/config-real.yaml | 6 ++++++ data/vector/robot-fast/config-real.yaml | 6 ++++++ data/vector/sofa-fast/config-real.yaml | 6 ++++++ docs/details/config_template_note.md | 6 ++++++ 44 files changed, 256 insertions(+) diff --git a/config/ikalibr-config.yaml b/config/ikalibr-config.yaml index abd686e..f786dcc 100644 --- a/config/ikalibr-config.yaml +++ b/config/ikalibr-config.yaml @@ -141,11 +141,17 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters # by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-01/config-real.yaml b/data/li-calib/Court-01/config-real.yaml index 7c896fe..292cf59 100644 --- a/data/li-calib/Court-01/config-real.yaml +++ b/data/li-calib/Court-01/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-02/config-real.yaml b/data/li-calib/Court-02/config-real.yaml index ac2aff3..c55589f 100644 --- a/data/li-calib/Court-02/config-real.yaml +++ b/data/li-calib/Court-02/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-03/config-real.yaml b/data/li-calib/Court-03/config-real.yaml index 5ebebb1..0b88607 100644 --- a/data/li-calib/Court-03/config-real.yaml +++ b/data/li-calib/Court-03/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-04/config-real.yaml b/data/li-calib/Court-04/config-real.yaml index 9fafbf1..486bd3c 100644 --- a/data/li-calib/Court-04/config-real.yaml +++ b/data/li-calib/Court-04/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-05/config-real.yaml b/data/li-calib/Court-05/config-real.yaml index f1d7c87..c7504e8 100644 --- a/data/li-calib/Court-05/config-real.yaml +++ b/data/li-calib/Court-05/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-01/config-real.yaml b/data/li-calib/Garage-01/config-real.yaml index 8ba896a..c65b4f4 100644 --- a/data/li-calib/Garage-01/config-real.yaml +++ b/data/li-calib/Garage-01/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-02/config-real.yaml b/data/li-calib/Garage-02/config-real.yaml index 3e29fc4..2b44eb3 100644 --- a/data/li-calib/Garage-02/config-real.yaml +++ b/data/li-calib/Garage-02/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-03/config-real.yaml b/data/li-calib/Garage-03/config-real.yaml index 918aed7..58b1d66 100644 --- a/data/li-calib/Garage-03/config-real.yaml +++ b/data/li-calib/Garage-03/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-04/config-real.yaml b/data/li-calib/Garage-04/config-real.yaml index 5dfe8fb..d5c2247 100644 --- a/data/li-calib/Garage-04/config-real.yaml +++ b/data/li-calib/Garage-04/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-05/config-real.yaml b/data/li-calib/Garage-05/config-real.yaml index 675b514..38f16d5 100644 --- a/data/li-calib/Garage-05/config-real.yaml +++ b/data/li-calib/Garage-05/config-real.yaml @@ -65,10 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml index 9894401..75358c1 100644 --- a/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml @@ -86,6 +86,8 @@ Configor: OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml index c5de1f7..cbc065e 100644 --- a/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml @@ -86,6 +86,8 @@ Configor: OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml index 9cbbfa8..9d67871 100644 --- a/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml @@ -66,10 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml index ca5212d..e3d4919 100644 --- a/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml @@ -66,10 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml index c7e35ec..de2341d 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml @@ -66,10 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml index 51526a7..e3410fd 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml @@ -84,10 +84,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml index 7e41dd8..9121eae 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml @@ -84,10 +84,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml index 4e9c557..08fe9c3 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml index e68c9b3..ea1b427 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml @@ -80,10 +80,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml index f479105..83e0ce3 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml @@ -82,10 +82,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml index 9eca647..4318c8e 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml @@ -82,10 +82,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml index 645aa60..5ccf7c0 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml @@ -66,10 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml index 8889505..6109814 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml @@ -66,10 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml index 108611d..6d5d64c 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml @@ -78,10 +78,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392614595/config-real.yaml b/data/river/data_202392614595/config-real.yaml index 021af87..a76c252 100644 --- a/data/river/data_202392614595/config-real.yaml +++ b/data/river/data_202392614595/config-real.yaml @@ -57,10 +57,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392615561/config-real.yaml b/data/river/data_202392615561/config-real.yaml index c30c670..d81f0fa 100644 --- a/data/river/data_202392615561/config-real.yaml +++ b/data/river/data_202392615561/config-real.yaml @@ -57,10 +57,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392616822/config-real.yaml b/data/river/data_202392616822/config-real.yaml index 6418745..188814e 100644 --- a/data/river/data_202392616822/config-real.yaml +++ b/data/river/data_202392616822/config-real.yaml @@ -57,10 +57,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-1/config-real.yaml b/data/tum/rs-seq-1/config-real.yaml index 08b44f7..23f3f64 100644 --- a/data/tum/rs-seq-1/config-real.yaml +++ b/data/tum/rs-seq-1/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-10/config-real.yaml b/data/tum/rs-seq-10/config-real.yaml index f118154..9c438dd 100644 --- a/data/tum/rs-seq-10/config-real.yaml +++ b/data/tum/rs-seq-10/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-2/config-real.yaml b/data/tum/rs-seq-2/config-real.yaml index 85c65aa..aa36902 100644 --- a/data/tum/rs-seq-2/config-real.yaml +++ b/data/tum/rs-seq-2/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-3/config-real.yaml b/data/tum/rs-seq-3/config-real.yaml index 912b779..96451fd 100644 --- a/data/tum/rs-seq-3/config-real.yaml +++ b/data/tum/rs-seq-3/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-4/config-real.yaml b/data/tum/rs-seq-4/config-real.yaml index 78f5661..15ad729 100644 --- a/data/tum/rs-seq-4/config-real.yaml +++ b/data/tum/rs-seq-4/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-5/config-real.yaml b/data/tum/rs-seq-5/config-real.yaml index 5f0e78c..06a5ecb 100644 --- a/data/tum/rs-seq-5/config-real.yaml +++ b/data/tum/rs-seq-5/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-6/config-real.yaml b/data/tum/rs-seq-6/config-real.yaml index e394e70..0fb645a 100644 --- a/data/tum/rs-seq-6/config-real.yaml +++ b/data/tum/rs-seq-6/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-7/config-real.yaml b/data/tum/rs-seq-7/config-real.yaml index bb18e4c..3d3c5ba 100644 --- a/data/tum/rs-seq-7/config-real.yaml +++ b/data/tum/rs-seq-7/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-8/config-real.yaml b/data/tum/rs-seq-8/config-real.yaml index 6d49a36..1773b97 100644 --- a/data/tum/rs-seq-8/config-real.yaml +++ b/data/tum/rs-seq-8/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-9/config-real.yaml b/data/tum/rs-seq-9/config-real.yaml index edeb126..db4da6c 100644 --- a/data/tum/rs-seq-9/config-real.yaml +++ b/data/tum/rs-seq-9/config-real.yaml @@ -61,10 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/desk-fast/config-real.yaml b/data/vector/desk-fast/config-real.yaml index 718a0e5..14448b9 100644 --- a/data/vector/desk-fast/config-real.yaml +++ b/data/vector/desk-fast/config-real.yaml @@ -93,10 +93,16 @@ Configor: GravityNorm: 9.79 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/hdr-fast/config-real.yaml b/data/vector/hdr-fast/config-real.yaml index 9b75938..875fd4f 100644 --- a/data/vector/hdr-fast/config-real.yaml +++ b/data/vector/hdr-fast/config-real.yaml @@ -88,10 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/mountain-fast/config-real.yaml b/data/vector/mountain-fast/config-real.yaml index 49ac923..9d89b4e 100644 --- a/data/vector/mountain-fast/config-real.yaml +++ b/data/vector/mountain-fast/config-real.yaml @@ -88,10 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/robot-fast/config-real.yaml b/data/vector/robot-fast/config-real.yaml index 5926a8a..ede9d77 100644 --- a/data/vector/robot-fast/config-real.yaml +++ b/data/vector/robot-fast/config-real.yaml @@ -88,10 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/sofa-fast/config-real.yaml b/data/vector/sofa-fast/config-real.yaml index 3696f02..804a267 100644 --- a/data/vector/sofa-fast/config-real.yaml +++ b/data/vector/sofa-fast/config-real.yaml @@ -88,10 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/docs/details/config_template_note.md b/docs/details/config_template_note.md index a5cb583..c2764fd 100644 --- a/docs/details/config_template_note.md +++ b/docs/details/config_template_note.md @@ -162,11 +162,17 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters # by setting this field to 'false' OptTemporalParams: true # if true, the last batch optimization will also optimize IMU nonlinearities OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time