12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixing strength of bias constraints in IMUKittiExampleGPS.cpp

release/4.3a0
Michal Nowicki 2024-03-18 08:47:11 +01:00
parent eeb092ec41
commit 865288a4dd
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/IMUKittiExampleGPS.cpp
View File

@ -241,7 +241,6 @@ int main(int argc, char* argv[]) {
"-- Starting main loop: inference is performed at each time step, but we " "-- Starting main loop: inference is performed at each time step, but we "
"plot trajectory every 10 steps\n"); "plot trajectory every 10 steps\n");
size_t j = 0; size_t j = 0;
size_t included_imu_measurement_count = 0;
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
// At each non=IMU measurement we initialize a new node in the graph // At each non=IMU measurement we initialize a new node in the graph
@ -249,6 +248,7 @@ int main(int argc, char* argv[]) {
auto current_vel_key = V(i); auto current_vel_key = V(i);
auto current_bias_key = B(i); auto current_bias_key = B(i);
double t = gps_measurements[i].time; double t = gps_measurements[i].time;
size_t included_imu_measurement_count = 0;
if (i == first_gps_pose) { if (i == first_gps_pose) {
// Create initial estimate and prior on initial pose, velocity, and biases // Create initial estimate and prior on initial pose, velocity, and biases