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

fix the included_imu_measurement_count scope

release/4.3a0
Varun Agrawal 2021-10-21 18:45:57 -04:00
parent 9aa0dbf493
commit b3e8bf2325
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
python/gtsam/examples/IMUKittiExampleGPS.py
View File

@ -219,7 +219,10 @@ def optimize(gps_measurements: List[GpsMeasurement],
# (3) we solve the graph to obtain and optimal estimate of robot trajectory # (3) we solve the graph to obtain and optimal estimate of robot trajectory
print("-- Starting main loop: inference is performed at each time step, " print("-- Starting main loop: inference is performed at each time step, "
"but we plot trajectory every 10 steps") "but we plot trajectory every 10 steps")
j = 0 j = 0
included_imu_measurement_count = 0
for i in range(first_gps_pose, len(gps_measurements)): for i in range(first_gps_pose, len(gps_measurements)):
# 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
current_pose_key = X(i) current_pose_key = X(i)
@ -246,7 +249,6 @@ def optimize(gps_measurements: List[GpsMeasurement],
current_summarized_measurement = gtsam.PreintegratedImuMeasurements( current_summarized_measurement = gtsam.PreintegratedImuMeasurements(
imu_params, current_bias) imu_params, current_bias)
included_imu_measurement_count = 0
while (j < len(imu_measurements) while (j < len(imu_measurements)
and imu_measurements[j].time <= t): and imu_measurements[j].time <= t):
if imu_measurements[j].time >= t_previous: if imu_measurements[j].time >= t_previous: