fixed bootstrapping

src/processor/processor_imu_3d.cpp

@@ -318,8 +318,15 @@ void ProcessorImu3d::bootstrap()
             // ALL TIMES:
             // To all frames not processed yet:
             // Fix zero velocity, initialize zero PO, and add zero 3D relative pose factors
-            while (bootstrap_last_frame_ != getProblem()->getLastFrame())
+            while (bootstrap_last_frame_ != getProblem()->getLastFrame() and getProblem()->getLastFrame())
             {
+                // first frame
+                if (not bootstrap_last_frame_)
+                    bootstrap_last_frame_ = first_capture->getFrame();
+                else
+                    // next
+                    bootstrap_last_frame_ = bootstrap_last_frame_->getNextFrame();
+
                 // emplace missing states
                 getProblem()->emplaceStatesToFrame(bootstrap_last_frame_, getProblem()->stateZero(getStateKeys()));
 
@@ -328,7 +335,8 @@ void ProcessorImu3d::bootstrap()
                 bootstrap_last_frame_->getV()->fix();
 
                 // Initialize state as same as first frame
-                bootstrap_last_frame_->setState(first_capture->getFrame()->getState("PO"));
+                if (first_capture->getFrame() != bootstrap_last_frame_)
+                    bootstrap_last_frame_->setState(first_capture->getFrame()->getState("PO"));
 
                 // Factor zero-relative pose
                 if (bootstrap_last_frame_->getPreviousFrame())
@@ -348,13 +356,10 @@ void ProcessorImu3d::bootstrap()
                                                               false,
                                                               TOP_MOTION);
                 }
-
-                // next
-                bootstrap_last_frame_ = bootstrap_last_frame_->getNextFrame();
             }
 
             // ONLY AFTER bootstrap_averaging_length_ seconds:
-            // Orientation initialization aligning with g
+            // To all problem: Orientation initialization aligning with g
             if (t_current - first_capture->getTimeStamp() >= bootstrap_averaging_length_)
             {
                 // get initial IMU frame 's' expressed in local world frame 'l'