diff --git a/aruco/ArucoDetector.cpp b/aruco/ArucoDetector.cpp
index c8ff7fa..01ed71e 100644
--- a/aruco/ArucoDetector.cpp
+++ b/aruco/ArucoDetector.cpp
@@ -9,6 +9,7 @@ ArucoDetector::ArucoDetector(Type::RobotPose* pose, const std::string& calibrati
 
     // 4.6
     this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
+    this->parameters = cv::aruco::DetectorParameters::create();
 
 
     this->transformationMatrix = (cv::Mat_<double>(4, 4) <<
@@ -84,9 +85,11 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
 
     cv::Mat frame;
     cv::Mat frameNotRotated;
+    cv::Mat frameDistored;
     cam->getVideoFrame(frameNotRotated, 1000);
     // cap >> frame;  // Capture frame from the camera
-    cv::flip(frameNotRotated, frame, -1);
+    cv::flip(frameNotRotated, frameDistored, -1);
+    cv::undistort(frameDistored, fram, cameraMatrix, distCoeffs);
 
     std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> result;
 
@@ -102,7 +105,7 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
     std::vector<std::vector<cv::Point2f>> markerCorners;
 
     // 4.6
-    cv::aruco::detectMarkers(frame, this->dictionary, markerCorners, markerIds);
+    cv::aruco::detectMarkers(frame, this->dictionary, markerCorners, markerIds, );
 
     // opencv 4.8
     // detector.detectMarkers(frame, markerCorners, markerIds);
@@ -127,7 +130,7 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
 
             cv::Mat rvec, tvec;
 
-            solvePnP(tag.objectRepresenation, markerCorners.at(i), cameraMatrix, distCoeffs, rvec, tvec);
+            solvePnP(tag.objectRepresenation, markerCorners.at(i), cameraMatrix, distCoeffs, rvec, tvec, false, SOLVEPNP_IPPE);
 
             if (!headless)
             {
diff --git a/aruco/ArucoDetector.h b/aruco/ArucoDetector.h
index 4c1da3b..20c0cfc 100644
--- a/aruco/ArucoDetector.h
+++ b/aruco/ArucoDetector.h
@@ -17,6 +17,7 @@ class ArucoDetector {
 
     // 4.6
     cv::Ptr<cv::aruco::Dictionary> dictionary;
+    cv::Ptr<cv::aruco::DetectorParameters> parameters;
 
     // 4.8
     // cv::aruco::Dictionary dictionary;