really rough hack to work out when the camera soln has jumped due to BA

Examples/Monocular/mono_cland.cc

@@ -27,7 +27,7 @@
 #include<opencv2/core.hpp>
 
 #include"System.h"
-
+#include"mavlink/common/mavlink.h"
 using namespace std;
 
 void LoadImages(const string &strSequence, vector<string> &vstrImageFilenames,
@@ -61,7 +61,14 @@ int main(int argc, char **argv)
 
     // Main loop
     cv::Mat im;
+
     cv::Mat mIFrameTransRot;
+    int trackingStatus = -1;
+    bool cameraHasJumped = false;
+
+    cv::Mat cameraPose = cv::Mat::eye(4,4,CV_32F);
+    cv::Mat cameraRotation = cv::Mat::eye(3,3,CV_32F);
+    cv::Mat cameraTranslation(1,3,CV_32F);
 
     for(int ni=0; ni<nImages; ni++)
     {
@@ -71,11 +78,10 @@ int main(int argc, char **argv)
         // Supply optional inter-frame rotation and translation
         // TODO: pull this info from the Autopilot EKF and produce the matrix
         mIFrameTransRot = cv::Mat(); //cv::Mat::eye(4,4,CV_32F);
-
-        double tframe = vTimestamps[ni];
         // if we set the mIFrameTransRot to something other than empty it will be used for mVelocity in Tracking.cc.
         // we need to set it to be the rotation and translation between this frame and the last in camera world coordinate system.
-
+        // TODO: ask about the best method to do this step
+        double tframe = vTimestamps[ni];
 
         if(im.empty())
         {
@@ -90,15 +96,26 @@ int main(int argc, char **argv)
 #endif
 
         // Pass the image to the SLAM system
-        cv::Mat cameraPose = SLAM.TrackMonocular(im,tframe,mIFrameTransRot);
-
-//        if (!cameraPose.empty())
-//        {
-//        	//TODO: extract rotation matrix (first three col and rows)
-//        	//TODO: extract translation (last col, first three rows)
-//        	//TODO: convert to EKF NED coordinate system
-//        	//TODO: send back to Autopilot for EKF fusion
-//        }
+
+
+        cameraPose = SLAM.TrackMonocular(im,tframe,mIFrameTransRot);
+        trackingStatus = SLAM.GetStatus();
+        cameraHasJumped = SLAM.CameraHasJumped();
+
+        if (!cameraPose.empty())
+        {
+//        	cout << "cameraPose: " << cameraPose << endl << endl;
+        	cameraPose.rowRange(0,3).colRange(0,3).copyTo(cameraRotation.rowRange(0,3).colRange(0,3));
+        	//extract rotation matrix (first three col and rows)
+//        	cout << "cameraRotation: " << cameraRotation << endl << endl;
+        	cameraPose.rowRange(0,3).col(3).copyTo(cameraTranslation);
+        	//extract translation (last col, first three rows)
+        	cout << "cameraTranslation: " << cameraTranslation << endl << endl;
+        	cout << "trackingStatus: " << trackingStatus << endl << endl;
+        	cout << "cameraHasJumped: " << cameraHasJumped << endl << endl;
+        	//TODO: convert to NED coordinate system and then a fake gps msg
+        	//TODO: send back to Autopilot for EKF fusion
+        }
 
 
 #ifdef COMPILEDWITHC11

include/LoopClosing.h

@@ -48,6 +48,7 @@ class LoopClosing
     typedef pair<set<KeyFrame*>,int> ConsistentGroup;    
     typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
         Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
+    bool optimizerRan = false;
 
 public:
 

include/System.h

@@ -55,6 +55,8 @@ class System
         STEREO=1,
         RGBD=2
     };
+    bool cameraHasJumped = false;
+    bool GBARunning = true;
 
 public:
 
@@ -90,6 +92,9 @@ class System
     // This function must be called before saving the trajectory.
     void Shutdown();
 
+    int GetStatus();
+    bool CameraHasJumped();
+
     // Save camera trajectory in the TUM RGB-D dataset format.
     // Call first Shutdown()
     // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset

include/Tracking.h

@@ -86,6 +86,9 @@ class Tracking
         LOST=3
     };
 
+    bool mapHasBeenOptimised = false;
+    bool matchJump = false;
+
     eTrackingState mState;
     eTrackingState mLastProcessedState;
 

src/LoopClosing.cc

@@ -564,6 +564,8 @@ void LoopClosing::CorrectLoop()
     // Optimize graph
     Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
 
+    optimizerRan = true;
+
     // Add loop edge
     mpMatchedKF->AddLoopEdge(mpCurrentKF);
     mpCurrentKF->AddLoopEdge(mpMatchedKF);
@@ -658,6 +660,7 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
         {
             cout << "Global Bundle Adjustment finished" << endl;
             cout << "Updating map ..." << endl;
+
             mpLocalMapper->RequestStop();
             // Wait until Local Mapping has effectively stopped
 
@@ -733,7 +736,7 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
             }
 
             mpLocalMapper->Release();
-
+            optimizerRan = true;
             cout << "Map updated!" << endl;
         }
 

src/Optimizer.cc

@@ -831,8 +831,8 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p
             VSim3->setEstimate(Siw);
         }
 
-        if(pKF==pLoopKF)
-            VSim3->setFixed(true);
+        if(pKF->mnId==0) //if(pKF==pLoopKF) //pKF->mnId==0
+        	VSim3->setFixed(true);
 
         VSim3->setId(nIDi);
         VSim3->setMarginalized(false);

src/System.cc

@@ -217,6 +217,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub
 
 cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const cv::Mat &mIFrameTransRot)
 {
+	cameraHasJumped = false;
     if(mSensor!=MONOCULAR)
     {
         cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
@@ -256,8 +257,44 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
         mbReset = false;
     }
     }
+    // if GBA was running last frame and now it is finished
+    if(GBARunning && mpLoopCloser->isFinishedGBA())
+    {
+    	// assume the camera has jumped
+    	cameraHasJumped = true;
+    }
+
+
+    GBARunning = mpLoopCloser->isRunningGBA();
+
+//    if(mpTracker->mapHasBeenOptimised)
+//	{
+//		cout << "mapHasBeenOptimised jump" << endl;
+//
+//	}
+
+//    if(mpTracker->matchJump)
+//	{
+//		cout << "match jump" << endl;
+//
+//	}
+
+
+    if(cameraHasJumped || mpLoopCloser->optimizerRan || mpTracker->matchJump || mpTracker->mapHasBeenOptimised)
+    {
+    	cameraHasJumped = true;
+    	//cout << "Camera has jumped!"<< endl;
+    }
+
 
+    if(mpLoopCloser->optimizerRan)
+    {
+//    	cout << "loop closer jump" << endl;
+    	mpLoopCloser->optimizerRan = false;
+
+    }
     return mpTracker->GrabImageMonocular(im,timestamp,mIFrameTransRot);
+
 }
 
 void System::ActivateLocalizationMode()
@@ -278,6 +315,16 @@ void System::Reset()
     mbReset = true;
 }
 
+int System::GetStatus()
+{
+    return mpTracker->mState;
+}
+
+bool System::CameraHasJumped()
+{
+	return cameraHasJumped;
+}
+
 void System::Shutdown()
 {
     mpLocalMapper->RequestFinish();

src/Tracking.cc

@@ -268,6 +268,9 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp,
 
 void Tracking::Track()
 {
+	mapHasBeenOptimised = false;
+	matchJump = false;
+
     if(mState==NO_IMAGES_YET)
     {
         mState = NOT_INITIALIZED;
@@ -438,9 +441,10 @@ void Tracking::Track()
 					mVelocity = mCurrentFrame.mTcw*LastTwc;
             	}
             }
-            else
+            else // this occurs on loop closure
                 mVelocity = cv::Mat();
 
+
             mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); //set the camera pose
 
             // Clean temporal point matches
@@ -696,6 +700,7 @@ void Tracking::CreateInitialMapMonocular()
     cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;
 
     Optimizer::GlobalBundleAdjustemnt(mpMap,20);
+    mapHasBeenOptimised = true;
 
     // Set median depth to 1
     float medianDepth = pKFini->ComputeSceneMedianDepth(2);
@@ -1508,6 +1513,7 @@ bool Tracking::Relocalization()
                 if(nGood>=50)
                 {
                     bMatch = true;
+                    matchJump = true;
                     break;
                 }
             }