diff --git a/ros2_ws/src/find-pose/src/FindObjectROS.cpp b/ros2_ws/src/find-pose/src/FindObjectROS.cpp
index 9d31fc97..b548a2ce 100644
--- a/ros2_ws/src/find-pose/src/FindObjectROS.cpp
+++ b/ros2_ws/src/find-pose/src/FindObjectROS.cpp
@@ -90,14 +90,39 @@ void FindObjectROS::estimate_pose()
     transform.header.frame_id =  "camera_link";
     // transform.header.stamp.sec = ros::Time::now();
     tf2::Quaternion q;
-
-    std::vector<cv::Point3f> objectPoints(4);
-    std::vector<cv::Point2f> imagePoints(4);
-
-    objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0);
-    objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0);
-    objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0);
-    objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0);
+    float depthConstant = 1.0f/641.587158203125;
+    std::vector<cv::Point3f> objectPoints(6);
+    std::vector<cv::Point2f> imagePoints(6);
+    cv::Vec3f point_1 = this->getDepth(depth,
+            246,665,
+            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+            1.0f/depthConstant, 1.0f/depthConstant);
+    cv::Vec3f point_2 = this->getDepth(depth,
+            618, 665,
+            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+            1.0f/depthConstant, 1.0f/depthConstant);
+    cv::Vec3f point_3 = this->getDepth(depth,
+            618, 111,
+            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+            1.0f/depthConstant, 1.0f/depthConstant);
+    cv::Vec3f point_4 = this->getDepth(depth,
+            246, 111,
+            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+            1.0f/depthConstant, 1.0f/depthConstant);
+    cv::Vec3f point_5 = this->getDepth(depth,
+            432, 386,
+            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+            1.0f/depthConstant, 1.0f/depthConstant);
+    cv::Vec3f point_6 = this->getDepth(depth,
+            537,224,
+            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+            1.0f/depthConstant, 1.0f/depthConstant);
+    objectPoints[0] = cv::Point3f(point_1.val[0], point_1.val[1], point_1.val[2]);
+    objectPoints[1] = cv::Point3f(point_2.val[0], point_2.val[1], point_2.val[2]);
+    objectPoints[2] = cv::Point3f(point_3.val[0], point_3.val[1], point_3.val[2]);
+    objectPoints[3] = cv::Point3f(point_4.val[0], point_4.val[1], point_4.val[2]);
+    objectPoints[4] = cv::Point3f(point_5.val[0], point_5.val[1], point_5.val[2]);
+    objectPoints[5] = cv::Point3f(point_6.val[0], point_6.val[1], point_6.val[2]);
 
     // QPointF pt = iter->map(QPointF(0, 0));
     imagePoints[0] = cv::Point2f(246,665);
@@ -107,8 +132,10 @@ void FindObjectROS::estimate_pose()
     imagePoints[2] = cv::Point2f(618,111);
     // pt = iter->map(QPointF(0, objectHeight));
     imagePoints[3] = cv::Point2f(246, 111);
+    imagePoints[4] = cv::Point2f(432, 386);
+    imagePoints[5] = cv::Point2f(537, 224);
+
 
-    float depthConstant = 1.0f/641.587158203125;
     cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
     cameraMatrix.at<double>(0,0) = 1.0f/depthConstant;
     cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
@@ -175,8 +202,8 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
 	float center_x = cx; //cameraInfo.K.at(2)
 	float center_y = cy; //cameraInfo.K.at(5)
 
-    std::cout << "cx: " << center_x << " cy: " << center_y << std::endl;
-    std::cout << "fx: " << fx << " fy: " << fy << std::endl;
+    // std::cout << "cx: " << center_x << " cy: " << center_y << std::endl;
+    // std::cout << "fx: " << fx << " fy: " << fy << std::endl;
 	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
 
 	// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
@@ -189,7 +216,7 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
 	bool isValid;
 	if(isInMM)
 	{
-        std::cout << "depth is in mm!!" << std::endl;
+        // std::cout << "depth is in mm!!" << std::endl;
 		depth = (float)depthImage.at<uint16_t>(y,x);
 		isValid = depth != 0.0f;
 	}
@@ -198,13 +225,14 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
         
 		depth = depthImage.at<float>(y,x);
 		isValid = std::isfinite(depth) && depth > 0.0f;
-        std::cout << "depth is NOT in mm!! " << depth << std::endl;
+        // std::cout << "depth is NOT in mm!! " << depth << std::endl;
 	}
 
 	// Check for invalid measurements
 	if (!isValid)
 	{
 		pt.val[0] = pt.val[1] = pt.val[2] = bad_point;
+                // std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl;
 	}
 	else
 	{
@@ -212,7 +240,7 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
 		pt.val[0] = (float(x) - center_x) * depth * constant_x;
 		pt.val[1] = (float(y) - center_y) * depth * constant_y;
 		pt.val[2] = depth*unit_scaling;
-        std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl;
+                std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl;
 	}
 	return pt;
 }