diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 0b0eed01ca67650158d47890b9294d8e99f9cab1..cf4a295c76d8608391181976e0bee78b0afb79cf 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -3,7 +3,10 @@
         {
             "name": "Linux",
             "includePath": [
-                "${workspaceFolder}/**"
+                "${workspaceFolder}/**",
+                "/usr/include",
+                "/usr/local/include",
+                "/usr/include/eigen3"
             ],
             "defines": [],
             "compilerPath": "/usr/bin/gcc",
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..298393367523370aa3c8f5098e9cfa316ec69441
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,53 @@
+{
+    "files.associations": {
+        "memory": "cpp",
+        "array": "cpp",
+        "string_view": "cpp",
+        "iosfwd": "cpp",
+        "deque": "cpp",
+        "vector": "cpp",
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "chrono": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "exception": "cpp",
+        "fstream": "cpp",
+        "functional": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "ostream": "cpp",
+        "ratio": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "system_error": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "type_traits": "cpp",
+        "tuple": "cpp",
+        "typeinfo": "cpp",
+        "utility": "cpp"
+    },
+    "C_Cpp.errorSquiggles": "Disabled"
+}
\ No newline at end of file
diff --git a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/CXX.includecache b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/CXX.includecache
index bc8c1ad87a35fa06620c7889fb64dbcb1ab51c88..258658ce700e47f3a648b6ad1aec8fd62450fb3b 100644
--- a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/CXX.includecache
+++ b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/CXX.includecache
@@ -111,6 +111,18 @@ cstdio
 -
 Eigen/SparseCholesky
 -
+opencv2/opencv.hpp
+-
+opencv2/core.hpp
+-
+opencv2/highgui.hpp
+-
+opencv2/imgproc.hpp
+-
+opencv2/tracking.hpp
+-
+opencv2/calib3d.hpp
+-
 
 /home/thomas/Documents/Minimisation/apps/specular_estimation/src/PhotometricStereo.h
 string
diff --git a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.internal b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.internal
index 70984491aafaa3d1234da3da784bc759989cc386..665e96b3e860a2f83aa618a8087c4b449ccc74aa 100644
--- a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.internal
+++ b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.internal
@@ -186,6 +186,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
  /usr/include/eigen3/Eigen/src/plugins/CommonCwiseUnaryOps.h
  /usr/include/eigen3/Eigen/src/plugins/MatrixCwiseBinaryOps.h
  /usr/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.h
+ /usr/local/include/opencv/cxcore.h
  /usr/local/include/opencv2/calib3d.hpp
  /usr/local/include/opencv2/core.hpp
  /usr/local/include/opencv2/core/affine.hpp
@@ -193,6 +194,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
  /usr/local/include/opencv2/core/bufferpool.hpp
  /usr/local/include/opencv2/core/check.hpp
  /usr/local/include/opencv2/core/core.hpp
+ /usr/local/include/opencv2/core/core_c.h
  /usr/local/include/opencv2/core/cuda.hpp
  /usr/local/include/opencv2/core/cuda.inl.hpp
  /usr/local/include/opencv2/core/cuda_types.hpp
@@ -215,6 +217,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
  /usr/local/include/opencv2/core/saturate.hpp
  /usr/local/include/opencv2/core/traits.hpp
  /usr/local/include/opencv2/core/types.hpp
+ /usr/local/include/opencv2/core/types_c.h
  /usr/local/include/opencv2/core/utility.hpp
  /usr/local/include/opencv2/core/version.hpp
  /usr/local/include/opencv2/core/vsx_utils.hpp
@@ -261,6 +264,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
  /usr/local/include/opencv2/imgcodecs.hpp
  /usr/local/include/opencv2/imgproc.hpp
  /usr/local/include/opencv2/imgproc/imgproc.hpp
+ /usr/local/include/opencv2/imgproc/types_c.h
  /usr/local/include/opencv2/ml.hpp
  /usr/local/include/opencv2/ml/ml.inl.hpp
  /usr/local/include/opencv2/objdetect.hpp
@@ -287,6 +291,12 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
  /usr/local/include/opencv2/stitching/warpers.hpp
  /usr/local/include/opencv2/superres.hpp
  /usr/local/include/opencv2/superres/optical_flow.hpp
+ /usr/local/include/opencv2/tracking.hpp
+ /usr/local/include/opencv2/tracking/feature.hpp
+ /usr/local/include/opencv2/tracking/onlineBoosting.hpp
+ /usr/local/include/opencv2/tracking/onlineMIL.hpp
+ /usr/local/include/opencv2/tracking/tldDataset.hpp
+ /usr/local/include/opencv2/tracking/tracker.hpp
  /usr/local/include/opencv2/video.hpp
  /usr/local/include/opencv2/video/background_segm.hpp
  /usr/local/include/opencv2/video/tracking.hpp
diff --git a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.make b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.make
index 8193b32d6841902ff6772f02aea7f9509fe150df..8f4eec03bb9dd9c9f67730b3d601da13dd562da9 100644
--- a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.make
+++ b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/depend.make
@@ -185,6 +185,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/include/eigen3/Eigen/src/plugins/CommonCwiseUnaryOps.h
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/include/eigen3/Eigen/src/plugins/MatrixCwiseBinaryOps.h
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.h
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv/cxcore.h
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/calib3d.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/affine.hpp
@@ -192,6 +193,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/bufferpool.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/check.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/core.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/core_c.h
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/cuda.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/cuda.inl.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/cuda_types.hpp
@@ -214,6 +216,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/saturate.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/traits.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/types.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/types_c.h
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/utility.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/version.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/core/vsx_utils.hpp
@@ -260,6 +263,7 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/imgcodecs.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/imgproc.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/imgproc/imgproc.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/imgproc/types_c.h
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/ml.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/ml/ml.inl.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/objdetect.hpp
@@ -286,6 +290,12 @@ apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStere
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/stitching/warpers.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/superres.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/superres/optical_flow.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/tracking.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/tracking/feature.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/tracking/onlineBoosting.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/tracking/onlineMIL.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/tracking/tldDataset.hpp
+apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/tracking/tracker.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/video.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/video/background_segm.hpp
 apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o: /usr/local/include/opencv2/video/tracking.hpp
diff --git a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o
index c8c898f9cf5cd3e18d65a6a481b12844b3994167..8b4c8931b816e21c609ca801275a55ce7c4ccc04 100644
Binary files a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o and b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/PhotometricStereo.cc.o differ
diff --git a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/specular_estimation.cc.o b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/specular_estimation.cc.o
index 431cadc3c2a6a9c64137ba4646f5f2c1b4be3fca..80c66f634a635ed0a908c65ecfb0930410af0266 100644
Binary files a/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/specular_estimation.cc.o and b/apps/specular_estimation/CMakeFiles/specular_estimation.dir/src/specular_estimation.cc.o differ
diff --git a/apps/specular_estimation/src/OpenCV.h b/apps/specular_estimation/src/OpenCV.h
index c34fa9095f31dfc50d17775c9df13512d6e065f5..1f8837de0d9f3e4e8bdbd1c73e860132843bc886 100644
--- a/apps/specular_estimation/src/OpenCV.h
+++ b/apps/specular_estimation/src/OpenCV.h
@@ -322,7 +322,7 @@ cv::Rect getBoundingBox(cv::Mat Mask) {
 }
 
 // Estimate the surface normals and p, q gradients
-void getSurfaceNormals(cv::Mat& normals, cv::Mat& pGradients, cv::Mat& qGradients, cv::Mat lightDirectionsInverted, std::vector< cv::Mat > modelImages) {
+void getSurfaceNormals(cv::Mat& normals, cv::Mat& pGradients, cv::Mat& qGradients, cv::Mat lightDirectionsInverted, std::vector<cv::Mat> modelImages) {
 
 	std::cout << "\nEstimating Surface Normals.\n";
 
@@ -570,7 +570,7 @@ void loadModelImages(std::vector<cv::Mat>& modelImages, std::vector<cv::Mat>& te
 	std::cout << std::endl;
 }
 
-cv::Mat getLightDirections(std::vector< cv::Mat > calibrationImages, cv::Rect calibrationBoundingBox) {
+cv::Mat getLightDirections(std::vector<cv::Mat> calibrationImages, cv::Rect calibrationBoundingBox) {
 
 	cv::Mat lightDirections(int(calibrationImages.size()), 3, CV_32F);
 
diff --git a/apps/specular_estimation/src/PhotometricStereo.cc b/apps/specular_estimation/src/PhotometricStereo.cc
index dad8621c7fbcb3bf5057183630bf2345257b4860..ceca521bd9ddfe9251eab7c40922583fd7430d12 100755
--- a/apps/specular_estimation/src/PhotometricStereo.cc
+++ b/apps/specular_estimation/src/PhotometricStereo.cc
@@ -7,6 +7,14 @@
 #include <cstdio>
 #include <Eigen/SparseCholesky>
 
+#include <opencv2/opencv.hpp>
+#include <opencv2/core.hpp> // Core contains the definitions of the basic building blocks of the OpenCV library
+#include <opencv2/highgui.hpp> // HighGUI contains the functions for input and output operations
+#include <opencv2/imgproc.hpp>
+#include <opencv2/tracking.hpp>
+#include <opencv2/calib3d.hpp>
+using namespace cv;
+
 namespace phoSte {
 
 namespace {
@@ -99,11 +107,41 @@ phoSte::light getLightDirection(const phoSte::circle& metalCircle,
     return phoSte::light(lx, ly, lz);
 }
 
+cv::Vec3f getLightDirectionFromSphere(cv::Mat Image, cv::Rect boundingBox) {
+
+	const int THRESH = 254;
+	const float radius = boundingBox.width / 2.0f;
+
+	cv::Mat Binary;
+	threshold(Image, Binary, THRESH, 255, CV_THRESH_BINARY);
+	cv::Mat SubImage(Binary, boundingBox);
+
+	// calculate center of pixels
+	cv::Moments m = moments(SubImage, false);
+	cv::Point center(int(m.m10 / m.m00), int(m.m01 / m.m00));
+
+	// x,y are swapped here // TODO check if necessary
+	float x = (center.y - radius) / radius;
+	float y = (center.x - radius) / radius;
+	float z = sqrt(1.0f - pow(x, 2.0f) - pow(y, 2.0f));
+
+	return cv::Vec3f(x, y, z);
+}
+
+phoSte::light getLightDirection(cv::Mat calibrationImage, cv::Rect calibrationBoundingBox) {
+
+    cv::Vec3f light = getLightDirectionFromSphere(calibrationImage, calibrationBoundingBox);
+    double lx = light[0];
+    double ly = light[1];
+    double lz = light[2];
+
+    return phoSte::light(lx, ly, lz);
+}
+
 void swapNum(double& a, double& b) {
     double tmp = a;
     a = b;
     b = tmp;
-
 }
 
 // find the num at the ratio of the nums
@@ -149,6 +187,7 @@ photometryStero::~photometryStero() {
     }
 }
 
+// Read in the file names for the images
 photometryStero::photometryStero(int n, int startI, int endI, std::string path,
                                  std::string metal1Phere1Name, std::string metal2Phere1Name,
                                  std::string lambertPhereName, std::string objectName,
@@ -172,6 +211,87 @@ photometryStero::photometryStero(int n, int startI, int endI, std::string path,
     mMaskNames.push_back(path + objectName);
 }
 
+photometryStero::photometryStero(int imageNumber, std::string modelPath, std::string calibrationPath, std::string macbethPath, std::string imageName, std::string calibration, double discardRatio): mDiscardRatio(discardRatio), imageNum(imageNumber) {
+    bool loadedImages = false;
+	imageNumber = 0;
+
+    // Load all of the model images
+	while (!loadedImages) {
+
+		// The index of the loop is converted into a stringstream, then to the string called "indexString"
+		// Using 'to_string' directly gives the error "'to_string' is not a member of 'std'"
+		std::ostringstream stm;
+		stm << imageNumber;
+		std::string indexString = stm.str();
+
+        std::string imageNameStr = modelPath + indexString + ".png";
+        cv::Mat modelImage = cv::imread(modelPath + indexString + ".png", cv::IMREAD_COLOR);
+
+        if (!modelImage.data) {  // Check if any images failed to load
+			std::cout << imageNumber << " model images have been loaded." << std::endl;
+			loadedImages = true;
+		}
+		else {
+            mImageNames.push_back(imageNameStr);
+			imageNumber++;
+		}
+    }
+
+    loadedImages = false;
+	imageNumber = 0;
+
+	// Load all of the calibration images
+	while (!loadedImages) {
+
+		// The index of the loop is converted into a stringstream, then to the string called "indexString"
+		// Using 'to_string' directly gives the error "'to_string' is not a member of 'std'"
+		std::ostringstream stm;
+		stm << imageNumber;
+		std::string indexString = stm.str();
+
+        std::string calibrationNameStr = calibrationPath + indexString + ".png";
+        cv::Mat calibrationImage = cv::imread(calibrationPath + indexString + ".png", cv::IMREAD_COLOR);
+
+        if (!calibrationImage.data) {  // Check if any images failed to load
+			std::cout << imageNumber << " calibration images have been loaded." << std::endl;
+			loadedImages = true;
+		}
+		else {
+            mCalibrationNames.push_back(calibrationNameStr);
+			imageNumber++;
+		}
+    }
+
+    std::string maskName = calibrationPath + "mask.png";
+    mMaskNames.push_back(maskName);
+
+    loadedImages = false;
+	imageNumber = 0;
+
+	// Load all of the calibration images
+	while (!loadedImages) {
+
+		// The index of the loop is converted into a stringstream, then to the string called "indexString"
+		// Using 'to_string' directly gives the error "'to_string' is not a member of 'std'"
+		std::ostringstream stm;
+		stm << imageNumber;
+		std::string indexString = stm.str();
+
+        std::string macbethNameStr = macbethPath + indexString + ".png";
+        cv::Mat macbethImage = cv::imread(macbethPath + indexString + ".png", cv::IMREAD_COLOR);
+
+        if (!macbethImage.data) {  // Check if any images failed to load
+			std::cout << imageNumber << " Macbeth images have been loaded." << std::endl;
+			loadedImages = true;
+		}
+		else {
+            mMacbethNames.push_back(macbethNameStr);
+			imageNumber++;
+		}
+    }
+    
+}
+
 // read image and mask image according to the name
 // return true if read successfully
 // return false if read fail
@@ -195,6 +315,12 @@ bool photometryStero::readImage() {
                          + 0.1140 * image.col(2));
         p2Image = p2Image.reshape(0, PFMAccessI->GetHeight());
         mp2Images.push_back(p2Image);
+
+        /*std::ostringstream stm;
+	    stm << i;
+	    std::string number = stm.str();
+        std::string path = "/home/thomas/Documents/Photometric-Stereo/Assignment_1/Apple/" + number + ".png";
+        cv::imwrite(path, p2Image);*/
     }
     // read the maskImage
     for(int i = 0; i < mMaskNames.size(); i++) {
@@ -206,6 +332,53 @@ bool photometryStero::readImage() {
     return true;
 }
 
+bool photometryStero::readImage(std::string modelPath, std::string calibrationPath, std::string macbethPath) {
+    if(mImageNames.empty() || mMaskNames.empty() || mCalibrationNames.empty() || mMacbethNames.empty())
+        return false;
+
+    // read the image
+    for(int i = 0; i < mImageNames.size(); i++) {
+        std::ostringstream stm;
+		stm << i;
+		std::string indexString = stm.str();
+
+        cv::Mat modelImage = cv::imread(modelPath + indexString + ".png", cv::IMREAD_COLOR);
+        mp2Images.push_back(modelImage);
+    }
+    // read the calibration image
+    for(int i = 0; i < mCalibrationNames.size(); i++) {
+        std::ostringstream stm;
+		stm << i;
+		std::string indexString = stm.str();
+
+        cv::Mat calibrationImage = cv::imread(calibrationPath + indexString + ".png", cv::IMREAD_COLOR);
+        mp2CalibrationImages.push_back(calibrationImage);
+    }
+    // read the macbeth image
+    for(int i = 0; i < mMacbethNames.size(); i++) {
+        std::ostringstream stm;
+		stm << i;
+		std::string indexString = stm.str();
+
+        cv::Mat macbethImage = cv::imread(macbethPath + indexString + ".png", cv::IMREAD_COLOR);
+        mp2MacbethImages.push_back(macbethImage);
+    }
+    // read the maskImage
+    //for(int i = 0; i < mMaskNames.size(); i++) {
+    //    cv::Mat p2Mask = cv::imread(calibrationPath + "mask.png", cv::IMREAD_GRAYSCALE);
+    //    if(! p2Mask.data )
+    //        return false;
+    //    mp2Mask.push_back(p2Mask);
+    //}
+
+    cv::Mat p2Mask = cv::imread(calibrationPath + "mask.png", cv::IMREAD_GRAYSCALE);
+    if(! p2Mask.data )
+        return false;
+    mp2Mask.push_back(p2Mask);
+
+    return true;
+}
+
 void photometryStero::getLightInformation(const int metalIndex, const int lambIndex) {
     // get the metal circle
     cv::threshold(mp2Mask[metalIndex], mp2Mask[metalIndex], 255 / 2, 255, cv::THRESH_BINARY);
@@ -235,7 +408,7 @@ void photometryStero::getLightInformation(const int metalIndex, const int lambIn
             mostPointIdx = i;
         }
     }
-    m_lambSpere = getCircle(lambContour[mostPointIdx]);
+    //m_lambSpere = getCircle(lambContour[mostPointIdx]);
 
     // get the direction and the intensity of every image
     for(int i = 0; i < mp2Images.size(); i++) {
@@ -248,11 +421,47 @@ void photometryStero::getLightInformation(const int metalIndex, const int lambIn
     }
 }
 
+void photometryStero::getLightInformation(const int metalIndex, const int lambIndex, cv::Rect boundingBox) {
+
+    m_metalSphere = phoSte::circle(boundingBox.x + (boundingBox.width/2), boundingBox.y + (boundingBox.height/2), boundingBox.width);
+
+    for(int i = 0; i < mp2Images.size(); i++) {
+
+        cv::Point metalMaxPoint;
+        cv::minMaxLoc(mp2Images[i], NULL, NULL, NULL, &metalMaxPoint, mp2Mask[metalIndex]);
+        m_light.push_back(getLightDirection(m_metalSphere, metalMaxPoint));
+
+        /*
+        const int THRESH = 254;
+        const float radius = boundingBox.width / 2.0f;
+
+        cv::Mat Binary;
+        threshold(mp2Images[i], Binary, THRESH, 255, CV_THRESH_BINARY);
+        cv::Mat SubImage(Binary, boundingBox);
+
+        // calculate center of pixels
+        cv::Moments m = moments(SubImage, false);
+        cv::Point center(int(m.m10 / m.m00), int(m.m01 / m.m00));
+
+        // x,y are swapped here // TODO check if necessary
+        float x = (center.y - radius) / radius;
+        float y = (center.x - radius) / radius;
+        float z = sqrt(1.0f - pow(x, 2.0f) - pow(y, 2.0f));
+
+        m_light.push_back(phoSte::light(x, y, z));
+        */
+
+        double lightIntensity;
+        cv::minMaxLoc(mp2Images[i], NULL, &lightIntensity, NULL, NULL, mp2Mask[lambIndex]);
+        m_light[i].mIntensity = lightIntensity;
+    }
+}
+
 // calculate the norm and albedo for every pixel
 // here, threshold is for the dark discard
 void photometryStero::getPixelNormAndAlbedo(const int objectIndex) {
     // calculate the pixel num
-    std::vector<double> pixelThreshold;
+    //std::vector<double> pixelThreshold;
     for(int i = 0; i < mp2Mask[objectIndex].rows; i++) {
         for(int j = 0; j < mp2Mask[objectIndex].cols; j++) {
             if(mp2Mask[objectIndex].at<unsigned char>(i, j) >= 255) {
@@ -261,6 +470,9 @@ void photometryStero::getPixelNormAndAlbedo(const int objectIndex) {
             }
         }
     }
+
+    std::cout << "Calculated the pixel coordinates within the mask\n";
+
     std::vector<std::vector<double>> allPixelValue;
     allPixelValue.reserve(imageNum);
     int objectPixelNum = mObjectX.size();
diff --git a/apps/specular_estimation/src/PhotometricStereo.h b/apps/specular_estimation/src/PhotometricStereo.h
index 9e6afb2e4bd2b630f9b53d5c2a9327d210cb4179..ad7019e7d77fbc643ae11c4c3aab1ba9fe498a01 100755
--- a/apps/specular_estimation/src/PhotometricStereo.h
+++ b/apps/specular_estimation/src/PhotometricStereo.h
@@ -44,15 +44,16 @@ struct light {
 class photometryStero {
   public:
     // fill the imageNames and maskNames
-    // use image with euqal distance
-    photometryStero(int n, int startI, int endI, std::string path,
-                    std::string metal1Phere1Name, std::string metal2Phere1Name,
-                    std::string lambertPhereName, std::string objectName, double discardRatio = 0.1);
+    // use image with equal distance
+    photometryStero(int n, int startI, int endI, std::string path, std::string metal1Phere1Name, std::string metal2Phere1Name, std::string lambertPhereName, std::string objectName, double discardRatio = 0.1);
+    photometryStero(int imageNumber, std::string modelPath, std::string calibrationPath, std::string macbethPath, std::string imageName, std::string calibration, double discardRatio = 0.1);
     photometryStero(const photometryStero&) = delete;
     photometryStero& operator = (const photometryStero&) = delete;
     ~photometryStero();
     bool readImage(); // read the images and masks according to the ImageNames
+    bool readImage(std::string modelPath, std::string calibrationPath, std::string macbethPath);
     void getLightInformation(const int metalIndex, const int lambIndex);
+    void getLightInformation(const int metalIndex, const int lambIndex, cv::Rect boundingBox);
     void getPixelNormAndAlbedo(const int objectIndex);
     cv::Mat outputNormalImage(int objectIndex);
     cv::Mat outputAlbedoImage(int objectIndex);
@@ -65,16 +66,20 @@ class photometryStero {
     void addSmallMaskForObject(int size, int midX, int midY);
   private:
     const int imageNum; // the num of images used to calculate;
-    std::vector<std::string> mImageNames; // the name of images
-    std::vector<std::string> mMaskNames; // the name of mask
+    std::vector<std::string> mImageNames;       // the file names of the model images
+    std::vector<std::string> mMaskNames;        // the file name of the mask image
+    std::vector<std::string> mCalibrationNames; // the file names of the chrome sphere images
+    std::vector<std::string> mMacbethNames;     // the file names of the Macbeth images
     std::vector<cv::Mat> mp2Images;
+    std::vector<cv::Mat> mp2CalibrationImages;
+    std::vector<cv::Mat> mp2MacbethImages;
     std::vector<cv::Mat> mp2Mask;
     // when the PFMACCess destructs, the data will be destroyed
     // so it's very dangerous, it should not be copied
     std::vector<CPFMAccess*> mImageStorage;
     // tht circle data for the metalSphere
     phoSte::circle m_metalSphere;
-    phoSte::circle m_lambSpere;
+    //phoSte::circle m_lambSpere;
     std::vector<phoSte::light> m_light;
     cv::Mat mN;
     cv::Mat mAlbedo;
@@ -82,6 +87,11 @@ class photometryStero {
     std::vector<int> mObjectY;
     std::vector<int> mInvalidIndex;
     const double mDiscardRatio;
+
+    std::string modelPath;
+    std::string calibrationPath;
+    std::string imageName;
+    std::string calibration;
 };
 }
 
diff --git a/apps/specular_estimation/src/specular_estimation.cc b/apps/specular_estimation/src/specular_estimation.cc
index 0646d2bbc7429016b11af84650ab210758b919c6..f77231f7e4ae87851c1d156a6337363b56705fbb 100644
--- a/apps/specular_estimation/src/specular_estimation.cc
+++ b/apps/specular_estimation/src/specular_estimation.cc
@@ -43,13 +43,6 @@ int main(int argc, char** argv) {
 
 	std::cout << std::endl;
 
-
-	drawAppleNormal();
-	drawAppleAlbedo();
-	drawAppleNormalWithAlbedo();
-	getAppleHeight();
-
-
 	// Define the paths for the model images, calibration images and the albedo texture
 	//const std::string sourcePath	  = "/user/HS222/tw00275/PhD/Minimisation/apps/specular_estimation/src/";
 	//const std::string imagesPath	  = "/media/tw00275/ESD-USB/";
@@ -58,20 +51,21 @@ int main(int argc, char** argv) {
 	const std::string folderPath	  = "2017-12-04";
 	const std::string modelPath       = imagesPath + folderPath + "/" + imageName   + "/" + imageName   + ".";
 	const std::string calibrationPath = imagesPath + folderPath + "/" + calibration + "/" + calibration + ".";
+	const std::string macbethPath	  = imagesPath + folderPath + "/macbeth/macbeth.";
 	const std::string texturePath     = modelPath + "texture.png";
 
-	detectMacbeth("/home/thomas/Documents/2017-12-04/macbeth/macbeth.0.png");
+	//detectMacbeth("/home/thomas/Documents/2017-12-04/macbeth/macbeth.0.png");
 
 	// A vector of Mats allow multiple images to be allocated to the same object
 	// textureImages are the full colour photographs that are used to create the texture
 	// modelImages are the greyscale version of textureImages which are used to calculate the surface normals
 	// calibrationImages contain the reflections of the chrome sphere, and are used to determine the light directions
 	// charucoImages contain the ChArUco board, and are used to calibrate the camera
-	std::vector< cv::Mat > textureImages, modelImages, calibrationImages, charucoImages;
+	std::vector<cv::Mat> textureImages, modelImages, calibrationImages, charucoImages;
 	cv::Mat heightMap, normalMap, texture, lightDirections;
 	cv::Rect calibrationBoundingBox;
 	int width, height;
-
+/*
 	// Load the textureImages, modelImages, calibrationImages and charucoImages, then determine the bounding box around the chrome sphere
 	loadImages(imageName, calibration, width, height, imageScale, textureImages, calibrationPath, calibrationImages, modelPath, modelImages, calibrationBoundingBox);
 
@@ -80,7 +74,7 @@ int main(int argc, char** argv) {
 	// The rotation matrix is created from the rotation and translation vectors, and describes the rotation of the ChArUco board
 	// The inverse of this transformation is applied to the light directions
 	// The perspective transform warps the model images to align them with the ChArUco board
-	std::vector< cv::Vec3d > rvecs, tvecs;
+	std::vector<cv::Vec3d> rvecs, tvecs;
 	cv::Mat rotationMatrix, rotationVector, charucoBoardImage, cameraMatrix, distortionCoefficients, perspectiveTransform, lightDirectionsPerspective = cv::Mat(numberOfLights, 3, CV_32F);
 	cv::Ptr<cv::aruco::CharucoBoard> charucoBoard;
 	
@@ -88,16 +82,180 @@ int main(int argc, char** argv) {
 
 	//charucoAlignment(charucoImages, textureImages, numberOfLights, width, height, rotationMatrix, rotationVector, perspectiveTransform, lightDirections, lightDirectionsPerspective, rvecs, tvecs, charucoBoard, charucoBoardImage, cameraMatrix, distortionCoefficients);
 
+
+	//rotationMatrix = 
+	//rotationVector = 
+	//perspectiveTransform = 
+	//lightDirectionsPerspective = 
+	//std::vector<cv::Vec3d>& rvecs
+	//std::vector<cv::Vec3d>& tvecs
+
+
+	photometricStereo(imageName, calibration, modelPath, imageScale, heightMap, normalMap, texture, lightDirections, lightDirectionsPerspective, textureImages, modelImages, calibrationImages, calibrationBoundingBox, width, height, rvecs, tvecs);
+*/
+
+	//phoSte::photometryStero B(21, 2, 22, "/home/thomas/Documents/Photometric-Stereo/Assignment_1/Apple/", "mask_dir_1.png", "mask_dir_2.png", "mask_I.png", "applemask.png");
+    //B.readImage();
+
+
+	phoSte::photometryStero A(0, modelPath, calibrationPath, macbethPath, imageName, calibration, 0.1);
+    A.readImage(modelPath, calibrationPath, macbethPath);
+	cv::Mat calibrationMask = loadCalibrationMask(calibrationPath, height, width);
+	calibrationBoundingBox = getBoundingBox(calibrationMask);
+    A.getLightInformation(1, 2, calibrationBoundingBox);
+	std::cout << "Got light information\n";
+    A.getPixelNormAndAlbedo(3);
+	std::cout << "Got Normal and Albedo\n";
+    cv::Mat result = A.outputNormalImage(3);
+    cv::imshow("apple normal image", result);
+    cv::imwrite(modelPath + "normal", result);
+    cv::waitKey(0);
+
+	int numberOfLights = 6;
+
+/*
+	int imageNum = numberOfLights;
+	int objectPixelNum = width;
+	std::vector<phoSte::light> m_light;
+	for (int i = 0; i < numberOfLights; i++) {
+		m_light.push_back(phoSte::light(lightDirections.at<float>(i, 0), lightDirections.at<float>(i, 1), lightDirections.at<float>(i, 2)));
+	}
+
+	cv::Mat I(imageNum, objectPixelNum, CV_64F);
+    cv::Mat L(imageNum, 3, CV_64F);
+    cv::Mat mN(3, objectPixelNum, CV_64F);
+    cv::Mat mAlbedo(1, objectPixelNum, CV_64F);
+    double *pL = (double *)L.data;
+    for(int i = 0; i < imageNum; i++) {
+        *pL = m_light[i].mx;
+        *(pL + 1) = m_light[i].my;
+        *(pL + 2) = m_light[i].mz;
+        pL += 3;
+    }
+    cv::Mat LPseudoInvert;
+    cv::invert(L, LPseudoInvert, cv::DECOMP_SVD);
+    double *pI = (double *)I.data;
+    double *pN = (double *)mN.data;
+
+    for(int i = 0; i < objectPixelNum; i++) {
+        int inValidNum = 0;
+        cv::Mat specificL;
+        for(int j = 0; j < imageNum; j++) {
+            //if(allPixelValue[j][i] < allThreshold[j]) {
+                inValidNum ++;
+                if(inValidNum == 1) {
+                    L.copyTo(specificL);
+                }
+                double *p2LRow = specificL.ptr<double>(j);
+                *(p2LRow) = 0;
+                *(p2LRow + 1) = 0;
+                *(p2LRow + 2) = 0;
+                *(pI + j * objectPixelNum + i) = 0;
+            } else {
+                *(pI + j * objectPixelNum + i) = allPixelValue[j][i] / m_light[j].mIntensity;
+            }
+            if(imageNum - inValidNum < 3) {
+                break;
+            }
+        }
+        if(inValidNum == 0) {
+            mN.col(i) = LPseudoInvert * I.col(i);
+            double nx = mN.col(i).at<double>(0, 0);
+            double ny = mN.col(i).at<double>(1, 0);
+            double nz = mN.col(i).at<double>(2, 0);
+            mAlbedo.at<double>(0, i) = sqrt(nx * nx + ny * ny + nz * nz);
+        } else if(imageNum - inValidNum >= 3) {
+            cv::Mat specificLPseudoInvert;
+            cv::invert(specificL, specificLPseudoInvert, cv::DECOMP_SVD);
+            mN.col(i) = specificLPseudoInvert * I.col(i);
+            double nx = mN.col(i).at<double>(0, 0);
+            double ny = mN.col(i).at<double>(1, 0);
+            double nz = mN.col(i).at<double>(2, 0);
+            mAlbedo.at<double>(0, i) = sqrt(nx * nx + ny * ny + nz * nz);
+        } else {
+            mN.at<double>(0, i) = 0;
+            mN.at<double>(1, i) = 0;
+            mN.at<double>(2, i) = 0;
+            mAlbedo.at<double>(0, i) = 0;
+            //mInvalidIndex.push_back(i);
+        }
+    }
+
+	//cv::Mat normal = A.outputNormalImage(3);
+	cv::Mat normal = cv::Mat::zeros(height, width, CV_32FC3);
+    //int nextInvalid = mInvalidIndex.empty() ? INT_MAX : mInvalidIndex[0];
+    int nextInvalidIndex = 0;
+    int invalidPixelNum = 0;
+    for (int i = 0; i < width; i++) {
+		for (int j = 0; j < height; j++) {
+			double nx = mN.at<double>(0, i);
+			double ny = mN.at<double>(1, i);
+			double nz = mN.at<double>(2, i);
+			if (i == nextInvalid) {
+				invalidPixelNum++;
+				normal.at<cv::Vec3f>(j, i)[0] = 0;
+				normal.at<cv::Vec3f>(j, i)[1] = 0;
+				normal.at<cv::Vec3f>(j, i)[2] = 0;
+				if (nextInvalidIndex + 1 == mInvalidIndex.size()) {
+					nextInvalid = INT_MAX;
+				} else {
+					nextInvalidIndex++;
+					nextInvalid = mInvalidIndex[nextInvalidIndex];
+				}
+			} else {
+				double rootsquareSum = sqrt(nx * nx + ny * ny + nz * nz);
+				float nxf = (1 + nx / rootsquareSum) / 2;
+				float nyf = (1 + ny / rootsquareSum) / 2;
+				float nzf = (1 + nz / rootsquareSum) / 2;
+				normal.at<cv::Vec3f>(j, i)[0] = nxf;
+				normal.at<cv::Vec3f>(j, i)[1] = nyf;
+				normal.at<cv::Vec3f>(j, i)[2] = nzf;
+			//}
+		}
+    }
+
+    cv::imshow("Surface Normals", normal);
+    //cv::imwrite("/home/thomas/Documents/Photometric-Stereo/Assignment_1/Apple/normal.jpg", normal);
+    cv::waitKey(0);*/
 /*
-	rotationMatrix = 
-	rotationVector = 
-	perspectiveTransform = 
-	lightDirectionsPerspective = 
-	std::vector< cv::Vec3d > & rvecs
-	std::vector< cv::Vec3d > & tvecs
+	cv::Mat albedo = A.outputAlbedoImage(3);
+	cv::imshow("Albedo", albedo);
+	cv::waitKey(0);
+
+	cv::Mat normalWithAlbedo = A.outputNormalWithAlbedo(3);
+    cv::imshow("Albedo with Surface Normals", normalWithAlbedo);
+    cv::waitKey(0);
+
+	cv::Mat heights = A.getHeightMap(2, -0.1);
+    //std::string fileName = "appleDepth.png";
+    //cv::imwrite(path + fileName, heightMap);
+    cv::imshow("Height Map", heights);
+    cv::waitKey(0);
 */
+	/*phoSte::photometryStero A(21, 2, 22, "/home/thomas/Documents/Photometric-Stereo/Assignment_1/Apple/", "mask_dir_1.png", "mask_dir_2.png", "mask_I.png", "applemask.png");
+    A.readImage();
+    A.getLightInformation(1, 2);
+    A.getPixelNormAndAlbedo(3);
+    cv::Mat normal = A.outputNormalImage(3);
+    cv::imshow("Surface Normals", normal);
+    //cv::imwrite("/home/thomas/Documents/Photometric-Stereo/Assignment_1/Apple/normal.jpg", normal);
+    cv::waitKey(0);
+
+	cv::Mat albedo = A.outputAlbedoImage(3);
+	cv::imshow("Albedo", albedo);
+	cv::waitKey(0);
+
+	cv::Mat normalWithAlbedo = A.outputNormalWithAlbedo(3);
+    cv::imshow("Albedo with Surface Normals", normalWithAlbedo);
+    cv::waitKey(0);
+
+	cv::Mat heights = A.getHeightMap(2, -0.1);
+    //std::string fileName = "appleDepth.png";
+    //cv::imwrite(path + fileName, heightMap);
+    cv::imshow("Height Map", heights);
+    cv::waitKey(0);
+	*/
 
-	photometricStereo(imageName, calibration, modelPath, imageScale, heightMap, normalMap, texture, lightDirections, lightDirectionsPerspective, textureImages, modelImages, calibrationImages, calibrationBoundingBox, width, height, rvecs, tvecs);
 
 	glm::vec3 position, lightInvDir;
 	glm::mat4 depthProjectionMatrix, depthViewMatrix, depthModelMatrix = glm::mat4(1.0), depthMVP, ModelMatrix = glm::mat4(1.0), MVP, ViewMatrix, depthBiasMVP, ProjectionMatrix;
@@ -114,7 +272,7 @@ int main(int argc, char** argv) {
 	std::vector<cv::Vec3d> residuals;
 	int lightNumber = 0;
 	bool calculateResidual = false, perspectiveProjection, shadowControl;
-	double totalResidual, residualValue, SpecularIntensity = 0.5d, SpecularPower = 2.0d;
+	double totalResidual, residualValue, SpecularIntensity = 0.5, SpecularPower = 2.0;
 	cv::Mat residualImage;
 	
 	std::vector<glm::vec3> lightInvDirs;
diff --git a/bin/specular_estimation b/bin/specular_estimation
index 72d5beae6606f1d9b39935c8ca4666a1a8edb9ee..15ab5db51d391e9fff6435947e5f090e28460d5e 100755
Binary files a/bin/specular_estimation and b/bin/specular_estimation differ