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 d6651f03bcb82f2d5d1468c5eb27e0f0ce8cb7c4..16f55e2cc1e11d30643d7b0d4e74fd30543826fb 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 2d8b26e789a4a145d9a436839be31c8e0ff2acf1..4b6b2a580f7c57df028c87234971476b581440b3 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/Charuco.h b/apps/specular_estimation/src/Charuco.h
index ac50bc0e18a21426a52e5579d041db8320643fa7..0008d4895223184f50636d7bff0717ce82f0a9c9 100644
--- a/apps/specular_estimation/src/Charuco.h
+++ b/apps/specular_estimation/src/Charuco.h
@@ -85,7 +85,7 @@ void charucoAlignment(std::vector<cv::Mat>& charucoImages, std::vector<cv::Mat>
 		if ((i % numberOfLights) == 0) { // Only estimate the pose for the first image in each board position
 
 			// Obtain the pose of the ChArUco boards
-			undistortCharuco(charucoBoard, materialImages[i], charucoBoardImage, undistortedCharuco, cameraMatrix, distortionCoefficients, rvec, tvec, perspectiveTransform, true);
+			undistortCharuco(charucoBoard, materialImages[i], charucoBoardImage, undistortedCharuco, cameraMatrix, distortionCoefficients, rvec, tvec, perspectiveTransform, false);
 
 
 			/*glm::vec3 eulerAngles(rvec[0] / (2 * M_PI), rvec[1] / (2 * M_PI), rvec[2] / (2 * M_PI));
@@ -173,7 +173,7 @@ std::vector<cv::Mat> charucoAlignment(std::vector<cv::Mat> materialImages, int n
 		if ((i % numberOfLights) == 0) { // Only estimate the pose for the first image in each board position
 
 			// Obtain the pose of the ChArUco boards
-			undistortCharuco(charucoBoard, materialImages[i], charucoBoardImage, undistortedCharuco, cameraMatrix, distortionCoefficients, rvec, tvec, perspectiveTransform, true);
+			undistortCharuco(charucoBoard, materialImages[i], charucoBoardImage, undistortedCharuco, cameraMatrix, distortionCoefficients, rvec, tvec, perspectiveTransform, false);
 
 
 			/*glm::vec3 eulerAngles(rvec[0] / (2 * M_PI), rvec[1] / (2 * M_PI), rvec[2] / (2 * M_PI));
@@ -268,7 +268,7 @@ std::vector<cv::Mat> charucoAlignment(std::vector<cv::Mat> materialImages, int n
 		if ((i % numberOfLights) == 0) { // Only estimate the pose for the first image in each board position
 
 			// Obtain the pose of the ChArUco boards
-			undistortCharuco(charucoBoard, materialImages[i], charucoBoardImage, undistortedCharuco, cameraMatrix, distortionCoefficients, rvec, tvec, perspectiveTransform, true);
+			undistortCharuco(charucoBoard, materialImages[i], charucoBoardImage, undistortedCharuco, cameraMatrix, distortionCoefficients, rvec, tvec, perspectiveTransform, false);
 
 
 			/*glm::vec3 eulerAngles(rvec[0] / (2 * M_PI), rvec[1] / (2 * M_PI), rvec[2] / (2 * M_PI));
@@ -329,10 +329,11 @@ std::vector<cv::Mat> charucoAlignment(std::vector<cv::Mat> materialImages, int n
 		//std::cout << "ChArUco board width: " << width << ", ChArUco board height: " << height << ", Border width: " << borderWidth << ", Window width: " << windowWidth << ", Window height: " << windowHeight << std::endl;
 
 		cv::Mat croppedWindow(undistortedCharuco, crop);
+		croppedWindow.convertTo(croppedWindow, CV_32F);
 		charucoImages.push_back(croppedWindow);
 
-		cv::imshow("Undistorted ChArUco", croppedWindow);
-		cv::waitKey(0);
+		//cv::imshow("Undistorted ChArUco", croppedWindow);
+		//cv::waitKey(0);
 	}
 
 	return charucoImages;
@@ -399,8 +400,10 @@ void undistortCharuco(cv::Ptr<cv::aruco::CharucoBoard> board, cv::Mat inputImage
 		cv::aruco::drawDetectedCornersCharuco(estimatedPose, detectedCharucoCorners, detectedCharucoIds, cv::Scalar(255, 0, 0));
 		//cv::aruco::drawDetectedCornersCharuco(boardCopy, charucoCorners, charucoIds, cv::Scalar(255, 0, 0));
 
-		cv::namedWindow("ChArUco Board", cv::WINDOW_NORMAL);
-		cv::imshow("ChArUco Board", boardImage);
+		if (displayUndistortedCharuco) {
+			cv::namedWindow("ChArUco Board", cv::WINDOW_NORMAL);
+			cv::imshow("ChArUco Board", boardImage);
+		}
 
 		/*cv::namedWindow( "Detected ChArUco Board", cv::WINDOW_NORMAL );
 		cv::imshow("Detected ChArUco Board", estimatedPose);
@@ -444,8 +447,10 @@ void undistortCharuco(cv::Ptr<cv::aruco::CharucoBoard> board, cv::Mat inputImage
 				cv::aruco::drawAxis(estimatedPose, cameraMatrix, distortionCoefficients, rvec, tvec, 0.44f);
 			}
 
-			cv::namedWindow("Estimated Pose", cv::WINDOW_NORMAL);
-			cv::imshow("Estimated Pose", estimatedPose);
+			if (displayUndistortedCharuco) {
+				cv::namedWindow("Estimated Pose", cv::WINDOW_NORMAL);
+				cv::imshow("Estimated Pose", estimatedPose);
+			}
 		}
 	}
 }
diff --git a/apps/specular_estimation/src/OpenCV.h b/apps/specular_estimation/src/OpenCV.h
index ef4078ba9b1b33cb07e693948e817d41eb2d4545..7c88d7cf0fb092c0aaeedc836769a182ff6fbf05 100644
--- a/apps/specular_estimation/src/OpenCV.h
+++ b/apps/specular_estimation/src/OpenCV.h
@@ -688,7 +688,8 @@ std::vector<cv::Mat> loadImages(std::string path, int imageScale = 1) {
 		std::string indexString = stm.str();
 
 		// Read the image of the current index
-		cv::Mat image = cv::imread(path + indexString + ".png", cv::IMREAD_COLOR);
+		//cv::Mat image = cv::imread(path + indexString + ".png", cv::IMREAD_COLOR);
+		cv::Mat image = cv::imread(path + indexString + ".png", cv::IMREAD_GRAYSCALE);
 
 		if (!image.data) {  // Check if any images failed to load
 			std::cout << imageNumber << " images have been loaded." << std::endl;
diff --git a/apps/specular_estimation/src/PhotometricStereo.cc b/apps/specular_estimation/src/PhotometricStereo.cc
index 261267b85ec35ad58a83026005d485902c99dd39..d96fde5b5decb1fe404381f9170de237d23c0371 100755
--- a/apps/specular_estimation/src/PhotometricStereo.cc
+++ b/apps/specular_estimation/src/PhotometricStereo.cc
@@ -149,15 +149,15 @@ int linearTransform(double max, double min, double value) {
 }
 
 photometricStereo::~photometricStereo() {
-    if(!mImageStorage.empty()) {
+    /*if(!mImageStorage.empty()) {
         for(int i = 0; i < mImageStorage.size(); i++) {
             delete mImageStorage[i];
         }
-    }
+    }*/
 }
 
 // Read in the file names for the images
-photometricStereo::photometricStereo(int n, int startI, int endI, std::string path,
+/*photometricStereo::photometricStereo(int n, int startI, int endI, std::string path,
                                  std::string metal1Phere1Name, std::string metal2Phere1Name,
                                  std::string lambertPhereName, std::string objectName,
                                  double discardRatio): mDiscardRatio(discardRatio), imageNum(n) {
@@ -178,10 +178,10 @@ photometricStereo::photometricStereo(int n, int startI, int endI, std::string pa
     mMaskNames.push_back(path + mask2Name);
     mMaskNames.push_back(path + mask3Name);
     mMaskNames.push_back(path + objectName);
-}
+}*/
 
 photometricStereo::photometricStereo(int imageNumber, std::string modelPath, std::string calibrationPath, std::string charucoPath, std::string macbethPath, std::string imageName, std::string calibration, double discardRatio): mDiscardRatio(discardRatio), imageNum(imageNumber) {
-    int imageScale = 2, numberOfLights = 6;
+    int imageScale = 1, numberOfLights = 6;
 
     /*cv::Mat cameraMatrix, distortionCoefficients, lightDirectionsPerspective;
 
@@ -229,7 +229,7 @@ photometricStereo::photometricStereo(int imageNumber, int imageScale, std::vecto
     //mp2MacbethImages = readImages(macbethPath);
     //std::cout << "Loaded macbeth images.\n";
 
-    int numberOfLights = mp2CalibrationImages.size();
+    int numberOfLights = imageNumber;
     width  = mp2Images[0].cols;
     height = mp2Images[0].rows;
 
@@ -352,7 +352,7 @@ photometricStereo::photometricStereo(int imageNumber, std::string modelPath, std
 // read image and mask image according to the name
 // return true if read successfully
 // return false if read fail
-bool photometricStereo::readImage() {
+/*bool photometricStereo::readImage() {
     if(mImageNames.empty() || mMaskNames.empty())
         return false;
 
@@ -372,13 +372,13 @@ bool photometricStereo::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++) {
         cv::Mat p2Mask(cv::imread(mMaskNames[i], cv::IMREAD_GRAYSCALE));
@@ -387,7 +387,7 @@ bool photometricStereo::readImage() {
         mp2Mask.push_back(p2Mask);
     }
     return true;
-}
+}*/
 
 std::vector<cv::Mat> photometricStereo::readImages(std::string path, int imageScale = 1) {
 
@@ -415,6 +415,7 @@ std::vector<cv::Mat> photometricStereo::readImages(std::string path, int imageSc
             if (imageScale != 1) {
 				cv::resize(image, image, cv::Size(), 1.0 / imageScale, 1.0 / imageScale, cv::INTER_AREA);
 			}
+            image.convertTo(image, CV_32F);
 			images.push_back(image);
 			imageNumber++;
 			//std::cout << "Loaded image " << imageNumber << std::endl;
@@ -695,8 +696,17 @@ void photometricStereo::getPixelNormAndAlbedo(const int objectIndex, cv::Mat lig
 void photometricStereo::getPixelNormAndAlbedo(const int objectIndex) {
     // calculate the pixel num
     //std::vector<double> pixelThreshold;
-    for(int i = 0; i < mp2Mask[objectIndex].rows; i++) {
+    /*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) {
+                mObjectX.push_back(j);
+                mObjectY.push_back(i);
+            }
+        }
+    }*/
+
+    for(int i = 0; i < mp2Images[0].rows; i++) {
+        for(int j = 0; j < mp2Images[0].cols; j++) {
             //if(mp2Mask[objectIndex].at<unsigned char>(i, j) == 255) {
                 mObjectX.push_back(j);
                 mObjectY.push_back(i);
diff --git a/apps/specular_estimation/src/PhotometricStereo.h b/apps/specular_estimation/src/PhotometricStereo.h
index f01ee36d631ea5af4c568640eaf5aac8679050a9..e09e3d927a9d2c2f7b97797b53c2d15dddd904c1 100755
--- a/apps/specular_estimation/src/PhotometricStereo.h
+++ b/apps/specular_estimation/src/PhotometricStereo.h
@@ -32,14 +32,14 @@ class photometricStereo {
   public:
     // fill the imageNames and maskNames
     // use image with equal distance
-    photometricStereo(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);
+    //photometricStereo(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);
     photometricStereo(int imageNumber, std::string modelPath, std::string calibrationPath, std::string charucoPath, std::string macbethPath, std::string imageName, std::string calibration, double discardRatio = 0.1);
     photometricStereo(std::vector<cv::Mat> mp2Images, std::vector<cv::Mat> mp2CalibrationImages, std::vector<cv::Mat> mp2Mask, int imageNumber = 0, double discardRatio = 0.1);
     photometricStereo(int imageNumber, int imageScale, std::vector<cv::Mat> materialImages, std::string calibrationPath, std::string macbethPath, std::string imageName, std::string calibration, double discardRatio = 0.1);
     photometricStereo(const photometricStereo&) = delete;
     photometricStereo& operator = (const photometricStereo&) = delete;
     ~photometricStereo();
-    bool readImage(); // read the images and masks according to the ImageNames
+    //bool readImage(); // read the images and masks according to the ImageNames
     bool readImage(std::string modelPath, std::string calibrationPath, std::string macbethPath);
     std::vector<cv::Mat> readImages(std::string path, int imageScale);
     void getLightInformation(const int metalIndex, const int lambIndex);
@@ -67,7 +67,7 @@ class photometricStereo {
     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;
+    //std::vector<CPFMAccess*> mImageStorage;
     // tht circle data for the metalSphere
     phoSte::circle m_metalSphere;
     //phoSte::circle m_lambSpere;
diff --git a/apps/specular_estimation/src/specular_estimation.cc b/apps/specular_estimation/src/specular_estimation.cc
index 08d509f7aade2e16f8b861691531ce41e2682269..d6ad5e8e9c84936a967706c56fcfa043e89a8c35 100644
--- a/apps/specular_estimation/src/specular_estimation.cc
+++ b/apps/specular_estimation/src/specular_estimation.cc
@@ -1,6 +1,7 @@
 #include "specular_estimation.h"
 
 void specularEstimation(std::string imageName, std::string calibration, double Roughness, double Metallic, double RoughnessSynthetic, double MetallicSynthetic, bool synthetic, bool denseSample);
+std::vector<cv::Mat> charucoImageAlignment(std::string materialPath, std::string charucoPath, int imageScale, int numberOfLights, cv::Mat& perspectiveTransform);
 /*void renderSynthetic(std::string imageName, std::string calibration, double Roughness, double Metallic, double RoughnessSynthetic, double MetallicSynthetic);
 void denseRealSample(std::string imageName, std::string calibration, double Roughness, double Metallic);
 void denseSyntheticSample(std::string imageName, std::string calibration, double Roughness, double Metallic);*/
@@ -68,23 +69,17 @@ void specularEstimation(std::string imageName, std::string calibration, double R
 	int numberOfLights = 6, width = 2091, height = 1626;
 
 	cv::Mat residualImage, albedo, normalMap, heightMap;
-	std::vector<cv::Mat> materialImages, charucoImages, calibrationImages, mp2Mask;
+	std::vector<cv::Mat> materialImages, calibrationImages;
 	cv::Mat lightDirections = (cv::Mat_<float>(6,3) << 0.447712, 0.138562, 0.883377, 0.228758, -0.106536, 0.967636, 0.1, 0.0705882, 0.99248, 0.000653595, -0.0718954, 0.997412, -0.139216, -0.12549, 0.982279, -0.494771, 0.115033, 0.861376);
 
 	if (!synthetic) {
 		
-		int originalWidth = width, originalHeight = height, imageScale = 2;
+		int originalWidth = width, originalHeight = height, imageScale = 1;
 		width  /= imageScale;
 		height /= imageScale;
 
-		cv::Mat cameraMatrix, distortionCoefficients, lightDirectionsPerspective;
-		calibrateCamera(charucoPath, cameraMatrix, distortionCoefficients);
-		std::cout << "Camera parameters obtained.\n";
-
 		cv::Mat perspectiveTransform;
-		materialImages = loadImages(materialPath, imageScale);
-		materialImages = charucoAlignment(materialImages, numberOfLights, perspectiveTransform, cameraMatrix, distortionCoefficients);
-		std::cout << "ChArUco alignment successful.\n";
+		materialImages = charucoImageAlignment(materialPath, charucoPath, imageScale, numberOfLights, perspectiveTransform);
 		
 		/*
 		//std::cout << "Loading material images.\n";
@@ -94,11 +89,6 @@ void specularEstimation(std::string imageName, std::string calibration, double R
 		numberOfLights = calibrationImages.size();
 		*/
 
-
-		//charucoAlignment(charucoPath, charucoImages, materialImages, numberOfLights, width, height, lightDirections, lightDirectionsPerspective, cameraMatrix, distortionCoefficients);
-		
-
-		//phoSte::photometricStereo PhotometricStereo(materialImages, calibrationImages, mp2Mask, numberOfLights, 0);
 		//phoSte::photometricStereo PhotometricStereo(numberOfLights, materialPath, calibrationPath, charucoPath, macbethPath, imageName, calibration, 0);
     	phoSte::photometricStereo PhotometricStereo(numberOfLights, imageScale, materialImages, calibrationPath, macbethPath, imageName, calibration, 0);
 
@@ -111,15 +101,22 @@ void specularEstimation(std::string imageName, std::string calibration, double R
 		std::cout << "Got Normal and Albedo\n";
 
 		normalMap = PhotometricStereo.outputNormalImage(1);
+		std::cout << "Got Normal Map\n";
 		cv::imshow("Normal Map", normalMap);
+		//cv::waitKey(0);
+		
 		albedo = PhotometricStereo.outputAlbedoImage(1);
-		cv::imshow("Albedo", albedo);
-		cv::Mat normalAlbedo = PhotometricStereo.outputNormalWithAlbedo(1);
-		cv::imshow("Normal with Albedo", normalAlbedo);
+		std::cout << "Got Albedo\n";
+		
+		//cv::Mat normalAlbedo = PhotometricStereo.outputNormalWithAlbedo(1);
+		//cv::imshow("Normal with Albedo", normalAlbedo);
 		//cv::Mat heightMap = PhotometricStereo.getHeightMap(2, -0.1);
 		//cv::imshow("Height Map", heightMap);
 		//cv::imwrite(materialPath + "normal", result);
 		heightMap = cv::Mat::zeros(albedo.size(), CV_8U);
+		std::cout << "Got Height Map\n";
+		//albedo = cv::Mat::zeros(height, width, CV_8U);
+		//std::cout << "Got Albedo\n";
 		//cv::waitKey(0);
 
 		//normalMap.convertTo(normalMap, CV_8UC1);
@@ -127,6 +124,11 @@ void specularEstimation(std::string imageName, std::string calibration, double R
 		
 		cv::cvtColor(albedo, albedo, CV_GRAY2BGR);
 		albedo.convertTo(albedo, CV_8UC3);
+		cv::imshow("Albedo", albedo);
+		cv::waitKey(0);
+
+		width  = albedo.cols;
+		height = albedo.rows;
 	} else {
 		albedo = cv::imread(albedoPath), normalMap = cv::imread(normalPath), heightMap = cv::Mat::zeros(albedo.size(), CV_8U);
 		normalMap = convertImageToNormals(normalMap);
@@ -215,6 +217,19 @@ void specularEstimation(std::string imageName, std::string calibration, double R
 
 	return;
 }
+
+std::vector<cv::Mat> charucoImageAlignment(std::string materialPath, std::string charucoPath, int imageScale, int numberOfLights, cv::Mat& perspectiveTransform) {
+	cv::Mat cameraMatrix, distortionCoefficients;
+	calibrateCamera(charucoPath, cameraMatrix, distortionCoefficients);
+	std::cout << "Camera parameters obtained.\n";
+	
+	std::vector<cv::Mat> materialImages = loadImages(materialPath, imageScale);
+	materialImages = charucoAlignment(materialImages, numberOfLights, perspectiveTransform, cameraMatrix, distortionCoefficients);
+	std::cout << "ChArUco alignment successful.\n";
+
+	return materialImages;
+}
+
 /*
 void renderSynthetic(std::string imageName, std::string calibration, double Roughness, double Metallic, double RoughnessSynthetic, double MetallicSynthetic) {
 
diff --git a/bin/specular_estimation b/bin/specular_estimation
index 3091b94ab90bcbbaefeafc3c2d4778e1e6991e38..48ed0a71b9fef56ee01e8224a04784f04cabe336 100755
Binary files a/bin/specular_estimation and b/bin/specular_estimation differ