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

fix gravity initialization for calibration

Browse Source
This commit is contained in:
Vladyslav Usenko 2019-10-29 15:35:42 +01:00
parent b7e9c2e4c9
commit 89a7296bfa
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/calibration/cam_imu_calib.cpp
View File

@ -445,16 +445,17 @@ void CamImuCalib::initOptimization() {
const auto cp_it = calib_init_poses.find(tcid); const auto cp_it = calib_init_poses.find(tcid);
if (cp_it != calib_init_poses.end()) { if (cp_it != calib_init_poses.end()) {
calib_opt->addPoseMeasurement( Sophus::SE3d T_a_i =
timestamp_ns, cp_it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse();
cp_it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse());
calib_opt->addPoseMeasurement(timestamp_ns, T_a_i);
if (!g_initialized) { if (!g_initialized) {
for (size_t i = 0; for (size_t i = 0;
i < vio_dataset->get_accel_data().size() && !g_initialized; i++) { i < vio_dataset->get_accel_data().size() && !g_initialized; i++) {
const basalt::AccelData &ad = vio_dataset->get_accel_data()[i]; const basalt::AccelData &ad = vio_dataset->get_accel_data()[i];
if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) { if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) {
g_a_init = cp_it->second.T_a_c.so3() * ad.data; g_a_init = T_a_i.so3() * ad.data;
g_initialized = true; g_initialized = true;
std::cout << "g_a initialized with " << g_a_init.transpose() std::cout << "g_a initialized with " << g_a_init.transpose()
<< std::endl; << std::endl;

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit 7847c4699607eaca370427ae4e2e840c5b502526 Subproject commit fa1a74dfa8f8400b8e1b260c6f6b4c9ce59ed776