Added hooks to supply velocity matrix and read back camera pose

Also changed a number of includes to match the openCV 3.1.0 style

Thirdparty/DBoW2/DBoW2/FClass.h

@@ -10,7 +10,7 @@
 #ifndef __D_T_FCLASS__
 #define __D_T_FCLASS__
 
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
 #include <vector>
 #include <string>
 

build.sh

@@ -17,11 +17,14 @@ make -j4
 
 cd ../../../
 
-echo "Uncompress vocabulary ..."
+#echo "Uncompress vocabulary ..."
 
-cd Vocabulary
-tar -xf ORBvoc.txt.tar.gz
-cd ..
+#cd Vocabulary
+#tar -xf ORBvoc.txt.tar.gz
+#cd ..
+
+#echo "Converting vocabulary to binary"
+#./tools/bin_vocabulary
 
 echo "Configuring and building ORB_SLAM2 ..."
 
@@ -32,5 +35,4 @@ make -j4
 
 cd ..
 
-echo "Converting vocabulary to binary"
-./tools/bin_vocabulary
+

include/Converter.h

@@ -21,7 +21,7 @@
 #ifndef CONVERTER_H
 #define CONVERTER_H
 
-#include<opencv2/core/core.hpp>
+#include<opencv2/core.hpp>
 
 #include<Eigen/Dense>
 #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"

include/Frame.h

@@ -55,7 +55,7 @@ class Frame
     Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
 
     // Constructor for Monocular cameras.
-    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
+    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const cv::Mat &mIFrameTransRot);
 
     // Extract ORB on the image. 0 for left image and 1 for right image.
     void ExtractORB(int flag, const cv::Mat &im);
@@ -128,6 +128,9 @@ class Frame
     // Far points are inserted as in the monocular case from 2 views.
     float mThDepth;
 
+    // Ardupilot EKF interframe translation and rotation matrix
+    cv::Mat mIFrameTransRot;
+
     // Number of KeyPoints.
     int N;
 

include/FrameDrawer.h

@@ -25,8 +25,8 @@
 #include "MapPoint.h"
 #include "Map.h"
 
-#include<opencv2/core/core.hpp>
-#include<opencv2/features2d/features2d.hpp>
+#include<opencv2/core.hpp>
+#include<opencv2/features2d.hpp>
 
 #include<mutex>
 

include/MapPoint.h

@@ -25,7 +25,7 @@
 #include"Frame.h"
 #include"Map.h"
 
-#include<opencv2/core/core.hpp>
+#include<opencv2/core.hpp>
 #include<mutex>
 
 namespace ORB_SLAM2

include/ORBmatcher.h

@@ -23,8 +23,8 @@
 #define ORBMATCHER_H
 
 #include<vector>
-#include<opencv2/core/core.hpp>
-#include<opencv2/features2d/features2d.hpp>
+#include<opencv2/core.hpp>
+#include<opencv2/features2d.hpp>
 
 #include"MapPoint.h"
 #include"KeyFrame.h"

include/PnPsolver.h

@@ -51,7 +51,7 @@
 #ifndef PNPSOLVER_H
 #define PNPSOLVER_H
 
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
 #include "MapPoint.h"
 #include "Frame.h"
 

include/System.h

@@ -24,7 +24,7 @@
 
 #include<string>
 #include<thread>
-#include<opencv2/core/core.hpp>
+#include<opencv2/core.hpp>
 
 #include "Tracking.h"
 #include "FrameDrawer.h"
@@ -75,7 +75,7 @@ class System
     // Proccess the given monocular frame
     // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
     // Returns the camera pose (empty if tracking fails).
-    cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp);
+    cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp, const cv::Mat &mIFrameTransRot);
 
     // This stops local mapping thread (map building) and performs only camera tracking.
     void ActivateLocalizationMode();

include/Tracking.h

@@ -22,8 +22,8 @@
 #ifndef TRACKING_H
 #define TRACKING_H
 
-#include<opencv2/core/core.hpp>
-#include<opencv2/features2d/features2d.hpp>
+#include<opencv2/core.hpp>
+#include<opencv2/features2d.hpp>
 
 #include"Viewer.h"
 #include"FrameDrawer.h"
@@ -60,7 +60,7 @@ class Tracking
     // Preprocess the input and call Track(). Extract features and performs stereo matching.
     cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double &timestamp);
     cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp);
-    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);
+    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp, const cv::Mat &mIFrameTransRot);
 
     void SetLocalMapper(LocalMapping* pLocalMapper);
     void SetLoopClosing(LoopClosing* pLoopClosing);

src/Frame.cc

@@ -39,7 +39,7 @@ Frame::Frame()
 Frame::Frame(const Frame &frame)
     :mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight),
      mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()),
-     mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys),
+     mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), mIFrameTransRot(frame.mIFrameTransRot), N(frame.N), mvKeys(frame.mvKeys),
      mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn),  mvuRight(frame.mvuRight),
      mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec),
      mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()),
@@ -171,9 +171,9 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeSt
 }
 
 
-Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
+Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const cv::Mat &mIFrameTransRot)
     :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
-     mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
+     mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mIFrameTransRot(mIFrameTransRot)
 {
     // Frame ID
     mnId=nNextId++;
@@ -261,9 +261,9 @@ void Frame::SetPose(cv::Mat Tcw)
 void Frame::UpdatePoseMatrices()
 { 
     mRcw = mTcw.rowRange(0,3).colRange(0,3);
-    mRwc = mRcw.t();
+    mRwc = mRcw.t(); //transpose == inverse for rotation matrix
     mtcw = mTcw.rowRange(0,3).col(3);
-    mOw = -mRcw.t()*mtcw;
+    mOw = -mRcw.t()*mtcw; // mOw == mtwc;
 }
 
 bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)

src/FrameDrawer.cc

@@ -21,8 +21,8 @@
 #include "FrameDrawer.h"
 #include "Tracking.h"
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
 
 #include<mutex>
 

src/ORBextractor.cc

@@ -54,10 +54,10 @@
 */
 
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgproc.hpp>
 #include <vector>
 
 #include "ORBextractor.h"

src/ORBmatcher.cc

@@ -22,8 +22,8 @@
 
 #include<limits.h>
 
-#include<opencv2/core/core.hpp>
-#include<opencv2/features2d/features2d.hpp>
+#include<opencv2/core.hpp>
+#include<opencv2/features2d.hpp>
 
 #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
 

src/PnPsolver.cc

@@ -54,7 +54,7 @@
 
 #include <vector>
 #include <cmath>
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
 #include "Thirdparty/DBoW2/DUtils/Random.h"
 #include <algorithm>
 

src/Sim3Solver.cc

@@ -23,7 +23,7 @@
 
 #include <vector>
 #include <cmath>
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
 
 #include "KeyFrame.h"
 #include "ORBmatcher.h"

src/System.cc

@@ -215,7 +215,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub
     return mpTracker->GrabImageRGBD(im,depthmap,timestamp);
 }
 
-cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
+cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const cv::Mat &mIFrameTransRot)
 {
     if(mSensor!=MONOCULAR)
     {
@@ -257,7 +257,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
     }
     }
 
-    return mpTracker->GrabImageMonocular(im,timestamp);
+    return mpTracker->GrabImageMonocular(im,timestamp,mIFrameTransRot);
 }
 
 void System::ActivateLocalizationMode()

src/Tracking.cc

@@ -21,8 +21,8 @@
 
 #include "Tracking.h"
 
-#include<opencv2/core/core.hpp>
-#include<opencv2/features2d/features2d.hpp>
+#include<opencv2/core.hpp>
+#include<opencv2/features2d.hpp>
 
 #include"ORBmatcher.h"
 #include"FrameDrawer.h"
@@ -235,7 +235,7 @@ cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const d
 }
 
 
-cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
+cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp, const cv::Mat &mIFrameTransRot) // could pass in EKF info here...
 {
     mImGray = im;
 
@@ -255,9 +255,11 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
     }
 
     if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
-        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
+    	// initial frame
+        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mIFrameTransRot); //add optional EKF pose info
     else
-        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
+    	// we are already running...
+        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mIFrameTransRot);
 
     Track();
 
@@ -423,15 +425,23 @@ void Tracking::Track()
             // Update motion model
             if(!mLastFrame.mTcw.empty())
             {
-                cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
-                mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
-                mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
-                mVelocity = mCurrentFrame.mTcw*LastTwc;
+            	if(!mCurrentFrame.mIFrameTransRot.empty()) // use the supplied inter-frame velocity info if available
+            	{
+            		mVelocity = mCurrentFrame.mIFrameTransRot;
+
+            	}
+            	else // assume linear rotation and translation velocity model
+            	{
+					cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
+					mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
+					mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
+					mVelocity = mCurrentFrame.mTcw*LastTwc;
+            	}
             }
             else
                 mVelocity = cv::Mat();
 
-            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
+            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); //set the camera pose
 
             // Clean temporal point matches
             for(int i=0; i<mCurrentFrame.N; i++)
@@ -607,6 +617,8 @@ void Tracking::MonocularInitialization()
             return;
         }
 
+        // Here we define the first point...
+
         cv::Mat Rcw; // Current Camera Rotation
         cv::Mat tcw; // Current Camera Translation
         vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
@@ -872,8 +884,8 @@ bool Tracking::TrackWithMotionModel()
     // Create "visual odometry" points
     UpdateLastFrame();
 
-    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
-
+    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw); //if the velocity model was better we would have moved a more sensible distance...
+    //^^ sets mCurrentFrame mTcw
     fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
 
     // Project points seen in previous frame