Add Camera class

5 files changed, 112 insertions, 2 deletions

diff --git a/Makefile b/Makefile
index b7f2864..eca55b2 100644
--- a/Makefile
+++ b/Makefile
@@ -1,8 +1,9 @@
-OBJECT=src/main.o src/body.o src/state.o src/feature.o
+OBJECT=src/main.o src/body.o src/state.o src/feature.o src/camera.o
 CXXFLAGS+=-O2 -march=native -std=c++11 -pedantic-errors
 #CXXFLAGS+=-g
-CXXFLAGS+=$(shell pkg-config --cflags eigen3)
+CXXFLAGS+=$(shell pkg-config --cflags eigen3 yaml-cpp)
 #CXXFLAGS+=$(shell pkg-config --cflags opencv)
+LIBS+=$(shell pkg-config --libs eigen3 yaml-cpp)
 #LIBS+=$(shell pkg-config --libs opencv)
 
 slam: ${OBJECT}
diff --git a/src/camera.cpp b/src/camera.cpp
new file mode 100644
index 0000000..a78746f
--- /dev/null
+++ b/src/camera.cpp
@@ -0,0 +1,60 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  camera.cpp
+ *
+ *    Description:  Class for camera
+ *
+ *        Version:  1.0
+ *        Created:  03/19/2017 09:39:58 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+#include "camera.h"
+#include <iostream>
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Camera
+ *      Method:  Camera
+ * Description:  constructor
+ *--------------------------------------------------------------------------------------
+ */
+Camera::Camera (const char *fin)
+{
+    YAML::Node config = YAML::LoadFile(fin);
+    _K(0,0) = BINNING*config["camera_matrix"]["data"][0].as<double>();
+    _K(0,1) = BINNING*config["camera_matrix"]["data"][1].as<double>();
+    _K(0,2) = BINNING*config["camera_matrix"]["data"][2].as<double>();
+    _K(1,0) = BINNING*config["camera_matrix"]["data"][3].as<double>();
+    _K(1,1) = BINNING*config["camera_matrix"]["data"][4].as<double>();
+    _K(1,2) = BINNING*config["camera_matrix"]["data"][5].as<double>();
+    _K(2,0) = BINNING*config["camera_matrix"]["data"][6].as<double>();
+    _K(2,1) = BINNING*config["camera_matrix"]["data"][7].as<double>();
+    _K(2,2) = config["camera_matrix"]["data"][8].as<double>();
+
+    _d[0] = config["distortion_coefficients"]["data"][0].as<double>();
+    _d[1] = config["distortion_coefficients"]["data"][1].as<double>();
+    _d[2] = config["distortion_coefficients"]["data"][2].as<double>();
+    _d[3] = config["distortion_coefficients"]["data"][3].as<double>();
+
+}  /* -----  end of method Camera::Camera  (constructor)  ----- */
+
+
+Matrix<double,3,3>
+Camera::K (  )
+{
+    return _K ;
+}		/* -----  end of method Camera::K  ----- */
+
+Vector4d
+Camera::d ( )
+{
+    return _d;
+}		/* -----  end of method Camera::d  ----- */
+
diff --git a/src/camera.h b/src/camera.h
new file mode 100644
index 0000000..9e5f736
--- /dev/null
+++ b/src/camera.h
@@ -0,0 +1,44 @@
+#ifndef  camera_INC
+#define  camera_INC
+#include <Eigen/Dense>
+#include <yaml-cpp/yaml.h>
+
+#define BINNING 0.5 // set the binning factor
+using Eigen::Matrix;
+using Eigen::Vector4d;
+/*
+ * =====================================================================================
+ *        Class:  Camera
+ *  Description:  Class for the camera.
+ * =====================================================================================
+ */
+class Camera
+{
+    public:
+        /* ====================  LIFECYCLE     ======================================= */
+        Camera (const char *fin);                             /* constructor */
+
+        /* ====================  ACCESSORS     ======================================= */
+        Matrix<double,3,3> K();
+        Vector4d d();
+
+        /* ====================  MUTATORS      ======================================= */
+
+        /* ====================  OPERATORS     ======================================= */
+
+    protected:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+    private:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+        Matrix<double,3,3> _K;
+        Vector4d _d;
+
+}; /* -----  end of class Camera  ----- */
+
+
+#endif   /* ----- #ifndef camera_INC  ----- */
diff --git a/src/main.cpp b/src/main.cpp
index f1f192c..6da1cb4 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -225,6 +225,9 @@ main(int argc, char **argv)
     timestamp dt{0,0};
     timestamp t_old{0,0};
 
+    // Open camera YAML
+    Camera cam(argv[1]);
+
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
diff --git a/src/main.h b/src/main.h
index 1f29309..b6d181e 100644
--- a/src/main.h
+++ b/src/main.h
@@ -5,9 +5,11 @@
 #include <cstdio>
 #include <cstring>
 #include <Eigen/Dense>
+#include <istream>
 #include <iostream>
 
 #include "body.h"
+#include "camera.h"
 #include "state.h"
 #include "types.h"