Fixed vo reset from guess (after being lost) not correctly updated if…

… it is still lost after auto reset countdown

rtabmap_odom/src/OdometryROS.cpp

@@ -905,10 +905,9 @@ void OdometryROS::mainLoop()
 					"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
 					header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec());
 		}
-		else
+		else if(--resetCurrentCount_>0)
 		{
 			NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
-			--resetCurrentCount_;
 		}
 
 		if(resetCurrentCount_ == 0 || tooOldPreviousData)
@@ -936,6 +935,11 @@ void OdometryROS::mainLoop()
 					odometry_->reset(tfPose);
 				}
 			}
+			// Keep resetting if the odometry cannot initialize in next updates (e.g., lack of features).
+			// This will make sure we keep updating to latest guess pose.
+			if(resetCurrentCount_ == 0) {
+				++resetCurrentCount_;
+			}
 		}
 	}