ThirdParty Sophus

CMakeLists.txt

@@ -24,6 +24,7 @@ PATHS /usr/local
 NO_DEFAULT_PATH)
 
 find_package(CUDA)
+find_package(Eigen3 REQUIRED)
 
 set(CMAKE_BUILD_TYPE Debug)
 
@@ -78,7 +79,9 @@ set(vi_slam_SOURCE_FILES
 
 include_directories(
   ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
   ${PROJECT_SOURCE_DIR}/include
+  ${PROJECT_SOURCE_DIR}/thirdparty
   #/usr/local/cuda-8.0/targets/x86_64-linux/include
   #${PROJECT_SOURCE_DIR}/include/thirdparty/akaze
   #${CUDASIFT}

include/CameraModel.hpp

@@ -21,7 +21,7 @@
 using namespace cv;
 using namespace std;
 
-namespace uw
+namespace vi
 {
 
 class CameraModel

include/Imu.hpp

@@ -70,6 +70,11 @@ class ImuFilterNode{
         vector <Quaterniond> quaternionWorld;
         vector <Point3d> rpyAnglesWorld; // orientacion del robot en rpy respecto al mundo 
         vector <Point3d> accelerationWorld; // celeracion del robot respecto al mundo
+
+        // Residuales
+        Point3d residualRPY;
+        Point3d residualPosition;
+        Point3d residualVelocity;
         double timeStep;
     private:
         double elapsed_filter;

include/VISystem.hpp

@@ -0,0 +1,80 @@
+#include <iostream>
+#include <fstream>
+#include "DataReader.hpp"
+#include "Visualizer.hpp"
+#include "Camera.hpp"
+#include "Imu.hpp"
+#include "Plus.hpp"
+#include "CameraModel.hpp"
+#include "opencv2/core.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/calib3d.hpp"
+#include <ctime>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <std_msgs/Int32.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Quaternion.h>
+
+
+// Eigen library
+#include <eigen3/Eigen/Core>
+
+// Sophus
+#include "sophus/se3.hpp"
+#include "sophus/so3.hpp"
+
+
+using namespace std;
+using namespace cv;
+
+namespace vi
+{
+class CameraModel;
+class VISystem
+{
+    public:
+        VISystem();
+        ~VISystem();
+        VISystem(int argc, char *argv[], int _start_index);
+
+        void InitializeSystem(string _outputPath, string _depthPath, string _calPath, Point3d _iniPosition, Point3d _iniVelocity, float _iniYaw);
+        // Gauss-Newton using Foward Compositional Algorithm - Using features
+        void Calibration(string _calibration_path);
+        void EstimatePoseFeatures(Frame* _previous_frame, Frame* _current_frame);
+        void CalculateROI();
+        void AddFrame(Mat _currentImage, vector <Point3d> _imuAngularVelocity, vector <Point3d> _imuAcceleration);
+        void AddFrame(Mat _currentImage, vector <Point3d> _imuAngularVelocity, vector <Point3d> _imuAcceleration, vector <Point3d> _gtRPY) ;
+        void FreeLastFrame();
+
+        bool initialized, distortion_valid , depth_available;
+        int  num_keyframes;
+        int num_max_keyframes;
+
+        Mat map1, map2;
+        int h, w, h_input, w_input;
+        float fx, fy, cx, cy;
+
+        Point3d position;
+        Point3d velocity;
+        Quateriond qOrientation;
+        Point3d RPYOrientation;
+        CameraModel* camera_model;
+
+        Camera camera;
+        Imu imuCore;
+        std::ofstream outputFile;
+        Mat K;
+        Rect ROI;
+
+        // Residuales IMU
+
+
+
+
+};
+
+}
+

src/Imu.cpp

@@ -219,6 +219,10 @@ void Imu::estimate()
     computePosition();
     computeAngularVelocity();
     computeAngularPosition();
+    residualRPY = rpyAnglesWorld[quaternionWorld.size()-1]-rpyAnglesWorld[0];
+    residualVelocity =  velocity;
+    residualPosition = position;
+
 }
 
 void Imu::estimate(vector <Point3d> gtRPY)
@@ -228,6 +232,9 @@ void Imu::estimate(vector <Point3d> gtRPY)
     computePosition();
     computeAngularVelocity();
     computeAngularPosition();
+    residualRPY = rpyAnglesWorld[quaternionWorld.size()-1]-rpyAnglesWorld[0];
+    residualVelocity = velocity;
+    residualPosition = position;
     /*initialVelocity.x = velocity.x;
     initialVelocity.y = velocity.y;
     initialVelocity.z = velocity.z;