Updated initialization, now it is able to start from non-static state.

Author: huaizheng

README.md

@@ -1,6 +1,6 @@
 # R-VIO
 
-R-VIO is an efficient, lightweight, **robocentric visual-inertial odometry** algorithm for consistent 3D motion tracking using only a monocular camera and a 6-axis IMU. Different from standard world-centric VINS algorithms which directly estimate absolute motion of the sensing platform with respect to a fixed, gravity-aligned, global frame of reference, R-VIO estimates the relative motion of higher accuracy with respect to a moving local frame (for example, IMU frame) and updates the global pose (orientation and position) estimate through composition. This algorithm is developed with the robocentric sliding-window filtering-based VIO framework that we originally proposed in our *IROS2018* paper and further extended in our recent *IJRR* paper:
+R-VIO is an efficient, lightweight, **robocentric visual-inertial odometry** algorithm for consistent 3D motion tracking using only a monocular camera and a 6-axis IMU. Different from standard world-centric VINS algorithms which directly estimate absolute motion of the sensing platform with respect to a fixed, gravity-aligned, global frame of reference, R-VIO estimates the relative motion of higher accuracy with respect to a moving, local frame (for example, IMU frame) and updates global pose (orientation and position) estimate through composition. This code is developed with the robocentric sliding-window filtering-based VIO framework that was originally proposed in our *IROS2018* paper and further extended in our recent *IJRR* paper:
 
 - Zheng Huai and Guoquan Huang, **Robocentric visual-inertial odometry**, *The International Journal of Robotics Research (IJRR)*, July 2019: [here](https://journals.sagepub.com/doi/10.1177/0278364919853361).
 

config/rvio_euroc.yaml

@@ -92,23 +92,20 @@ Tracker.EnableEqualizer: 1
 # Use Sampson error or not (RANSAC)
 Tracker.UseSampson: 1
 
-# Threshold for inlier (RANSAC)
-Tracker.nSampsonThrd: 1e-4
+# Error threshold for inlier (RANSAC)
+Tracker.nInlierThrd: 1e-4
 
 #--------------------------------------------------------------------------------------------
 # Initialization Parameters (tunable).
 #--------------------------------------------------------------------------------------------
 
-# Use initial alignment or not
-INI.EnableAlignment: 1
-
-# Time length of initialization [s]
-INI.nTimeLength: 1
-
 # Thresholds for moving detection [rad,m]
 INI.nThresholdAngle: 0.005
 INI.nThresholdDispl: 0.01
 
+# Use gravity alignment or not
+INI.EnableAlignment: 1
+
 # Record the outputs or not
 INI.RecordOutputs: 0
 

include/rvio/System.h

@@ -45,7 +45,7 @@ class System
 
     ~System();
 
-    void MonoVIO(const cv::Mat& im, const double& timestamp, const int& seq);
+    void MonoVIO(const cv::Mat& im, const double& timestamp);
 
     void PushImuData(ImuData* data) { mpSensorDatabase->PushImuData(data); }
 
@@ -55,15 +55,14 @@ class System
 
     double mnImuRate;
     double mnCamTimeOffset;
-    double mnInitTimeLength;
 
     int mnSlidingWindowSize;
     int mnMinCloneStates;
 
     bool mbEnableAlignment;
     bool mbRecordOutputs;
 
-    bool mbIsInitialized;
+    bool mbIsReady;
     bool mbIsMoving;
 
     double mnThresholdAngle;

package.xml

@@ -23,4 +23,3 @@
   <depend>eigen_conversions</depend>
 
 </package>
-

src/rvio/Ransac.cc

@@ -200,8 +200,7 @@ double Ransac::SampsonError(const Eigen::Vector3d& pt1,
     Eigen::Vector3d Fx1 = E*pt1;
     Eigen::Vector3d Fx2 = E.transpose()*pt2;
 
-    return (pow(pt2.transpose()*E*pt1,2))/
-           (pow(Fx1(0),2)+pow(Fx1(1),2)+pow(Fx2(0),2)+pow(Fx2(1),2));
+    return (pow(pt2.transpose()*E*pt1,2))/(pow(Fx1(0),2)+pow(Fx1(1),2)+pow(Fx2(0),2)+pow(Fx2(1),2));
 }
 
 

src/rvio/System.cc

@@ -82,13 +82,11 @@ System::System(const std::string& strSettingsFile)
     const int bRecordOutputs = fsSettings["INI.RecordOutputs"];
     mbRecordOutputs = bRecordOutputs;
 
-    mnInitTimeLength = fsSettings["INI.nTimeLength"];
-
     mnThresholdAngle = fsSettings["INI.nThresholdAngle"];
     mnThresholdDispl = fsSettings["INI.nThresholdDispl"];
 
-    mbIsInitialized = false;
     mbIsMoving = false;
+    mbIsReady = false;
 
     mpTracker = new Tracker(strSettingsFile);
     mpUpdater = new Updater(strSettingsFile);
@@ -108,49 +106,92 @@ System::~System()
 }
 
 
-void System::MonoVIO(const cv::Mat& im, const double& timestamp, const int& seq)
+void System::MonoVIO(const cv::Mat& im, const double& timestamp)
 {
     static int nCloneStates = 0;
-    static int nImuDataCount = 0;
-    static int nImageCountAfterMoving = 0;
+    static int nImageCountAfterInit = 0;
 
     std::list<ImuData*> lImuDataSeq;
     int nImuData = mpSensorDatabase->GetImuDataByTimeStamp(timestamp+mnCamTimeOffset, lImuDataSeq);
     if (nImuData==0)
         return;
 
-    nImuDataCount += nImuData;
+
+    /**
+     * Moving detection
+     */
+    if (!mbIsMoving)
+    {
+        Eigen::Vector3d angle;
+        Eigen::Vector3d displ;
+        angle.setZero();
+        displ.setZero();
+
+        for (std::list<ImuData*>::const_iterator lit=lImuDataSeq.begin();
+             lit!=lImuDataSeq.end(); ++lit)
+        {
+            Eigen::Vector3d w = (*lit)->AngularVel;
+            Eigen::Vector3d a = (*lit)->LinearAccel;
+            double dt = (*lit)->TimeInterval;
+
+            angle += dt*w;
+            displ += .5*pow(dt,2)*(a-mnGravity*a/a.norm());
+        }
+
+        // If the change is lager than the threshold
+        if (angle.norm()>mnThresholdAngle || displ.norm()>mnThresholdDispl)
+            mbIsMoving = true;
+    }
 
 
     /*
      * Initialization
      */
-    if (!mbIsInitialized)
+    if (!mbIsReady)
     {
         static Eigen::Vector3d wm = Eigen::Vector3d::Zero();
         static Eigen::Vector3d am = Eigen::Vector3d::Zero();
+        static int nImuDataCount = 0;
 
         while (!lImuDataSeq.empty())
         {
-            wm += (lImuDataSeq.front())->AngularVel;
-            am += (lImuDataSeq.front())->LinearAccel;
-            lImuDataSeq.pop_front();
+            if (!mbIsMoving)
+            {
+                wm += (lImuDataSeq.front())->AngularVel;
+                am += (lImuDataSeq.front())->LinearAccel;
+
+                lImuDataSeq.pop_front();
+                nImuDataCount++;
+            }
+            else
+            {
+                if (nImuDataCount==0)
+                {
+                    wm = (lImuDataSeq.front())->AngularVel;
+                    am = (lImuDataSeq.front())->LinearAccel;
+
+                    nImuDataCount = 1;
+                }
+                else
+                {
+                    wm = wm/nImuDataCount;
+                    am = am/nImuDataCount;
+                }
+
+                mbIsReady = true;
+                break;
+            }
         }
 
-        if (nImuDataCount>mnInitTimeLength*mnImuRate)
+        if (mbIsReady)
         {
-            wm = wm/nImuDataCount;
-            am = am/nImuDataCount;
-
             Eigen::Vector3d g = am;
             g.normalize();
-            Eigen::Vector3d gI = mnGravity*g;
-
-            Eigen::Matrix3d Rot;
 
+            Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
             if (mbEnableAlignment)
             {
-                // i. Align z-axis with g
+                // i. Align z-axis with gravity
                 Eigen::Vector3d zv = g;
 
                 // ii. Make x-axis perpendicular to z-axis
@@ -163,16 +204,20 @@ void System::MonoVIO(const cv::Mat& im, const double& timestamp, const int& seq)
                 yv.normalize();
 
                 // iv. The orientation of {G} in {R0}
-                Rot << xv, yv, zv;
+                Eigen::Matrix3d tempR;
+                tempR << xv, yv, zv;
+                R = tempR;
             }
-            else
-                Rot.setIdentity();
 
             xkk.setZero(26,1);
-            xkk.block(0,0,4,1) = RotToQuat(Rot);
+            xkk.block(0,0,4,1) = RotToQuat(R);
             xkk.block(7,0,3,1) = g;
-            xkk.block(20,0,3,1) = wm; // bg
-            xkk.block(23,0,3,1) = am-gI; // ba
+
+            if (nImuDataCount>1)
+            {
+                xkk.block(20,0,3,1) = wm; // bg
+                xkk.block(23,0,3,1) = am-mnGravity*g; // ba
+            }
 
             double dt = 1/mnImuRate;
 
@@ -191,46 +236,13 @@ void System::MonoVIO(const cv::Mat& im, const double& timestamp, const int& seq)
             Pkk(20,20) = nImuDataCount*dt*pow(msigmaGyroBias,2);
             Pkk(21,21) = nImuDataCount*dt*pow(msigmaAccelBias,2); // ba
             Pkk(22,22) = nImuDataCount*dt*pow(msigmaAccelBias,2);
-            Pkk(23,23) = nImuDataCount*dt*pow(msigmaAccelBias,2);
-
-            mbIsInitialized = true;
+            Pkk(23,23) = nImuDataCount*dt*pow(msigmaAccelBias,2); 
         }
-
-        if (!mbIsInitialized) return;
-    }
-
-
-    /**
-     * Static/Moving detection
-     */
-    if (!mbIsMoving)
-    {
-        Eigen::Vector3d angle;
-        Eigen::Vector3d displ;
-        angle.setZero();
-        displ.setZero();
-
-        for (std::list<ImuData*>::const_iterator lit=lImuDataSeq.begin();
-             lit!=lImuDataSeq.end(); ++lit)
-        {
-            Eigen::Vector3d w = (*lit)->AngularVel;
-            Eigen::Vector3d a = (*lit)->LinearAccel;
-            double dt = (*lit)->TimeInterval;
-
-            angle += dt*w;
-            displ += .5*pow(dt,2)*(a-mnGravity*a/a.norm());
-        }
-
-        // If the change is lager than the threshold
-        if (angle.norm()>mnThresholdAngle || displ.norm()>mnThresholdDispl)
-            // Is moving
-            mbIsMoving = true;
         else
-            // Is static
             return;
     }
 
-    nImageCountAfterMoving++;
+    nImageCountAfterInit++;
 
 
     /**
@@ -263,7 +275,7 @@ void System::MonoVIO(const cv::Mat& im, const double& timestamp, const int& seq)
     /**
      * State augmentation
      */
-    if (nImageCountAfterMoving>1)
+    if (nImageCountAfterInit>1)
     {
         if (nCloneStates<mnSlidingWindowSize)
         {

src/rvio/Tracker.cc

@@ -109,15 +109,15 @@ Tracker::Tracker(const std::string& strSettingsFile)
     mpCornerCluster = new CornerCluster(nImageHeight, nImageWidth, nGridSize);
 
     const int bUseSampson = fsSettings["Tracker.UseSampson"];
-    const double nInlierThreshold = fsSettings["Tracker.nSampsonThrd"];
+    const double nInlierThreshold = fsSettings["Tracker.nInlierThrd"];
     mpRansac = new Ransac(bUseSampson, nInlierThreshold);
 
     mbIsTheFirstImage = true;
 
     mLastImage = cv::Mat();
 
-    mTrackPub = mTrackerNode.advertise<sensor_msgs::Image>("/rvio/track", 2);
-    mNewerPub = mTrackerNode.advertise<sensor_msgs::Image>("/rvio/newer", 2);
+    mTrackPub = mTrackerNode.advertise<sensor_msgs::Image>("/rvio/track", 1);
+    mNewerPub = mTrackerNode.advertise<sensor_msgs::Image>("/rvio/newer", 1);
 }
 
 

src/rvio_mono.cc

@@ -55,7 +55,7 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
 {
     static int lastseq = -1;
     if ((int)msg->header.seq!=lastseq+1 && lastseq!=-1)
-        ROS_ERROR("Image message drop! curr seq: %d expected seq: %d.", msg->header.seq, lastseq+1);
+        ROS_DEBUG("Image message drop! curr seq: %d expected seq: %d.", msg->header.seq, lastseq+1);
     lastseq = msg->header.seq;
 
     cv_bridge::CvImageConstPtr cv_ptr;
@@ -69,15 +69,15 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
         return;
     }
 
-    mpSys->MonoVIO(cv_ptr->image, cv_ptr->header.stamp.toSec(), msg->header.seq);
+    mpSys->MonoVIO(cv_ptr->image, cv_ptr->header.stamp.toSec());
 }
 
 
 void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr& msg)
 {
     static int lastseq = -1;
     if ((int) msg->header.seq!=lastseq+1 && lastseq!=-1)
-        ROS_ERROR("IMU message drop! curr seq: %d expected seq: %d.", msg->header.seq, lastseq+1);
+        ROS_DEBUG("IMU message drop! curr seq: %d expected seq: %d.", msg->header.seq, lastseq+1);
     lastseq = msg->header.seq;
 
     Eigen::Vector3d angular_velocity;