apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

code updated to use 6 points

Browse Source
This commit is contained in:
Apoorva Gupta 2023-03-06 14:48:49 +05:30
parent 66f57f7011
commit 241656445f
1 changed files with 42 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
ros2_ws/src/find-pose/src/FindObjectROS.cpp
View File

@ -90,14 +90,39 @@ void FindObjectROS::estimate_pose()
transform.header.frame_id = "camera_link"; transform.header.frame_id = "camera_link";
// transform.header.stamp.sec = ros::Time::now(); // transform.header.stamp.sec = ros::Time::now();
tf2::Quaternion q; tf2::Quaternion q;
float depthConstant = 1.0f/641.587158203125;
std::vector<cv::Point3f> objectPoints(4); std::vector<cv::Point3f> objectPoints(6);
std::vector<cv::Point2f> imagePoints(4); std::vector<cv::Point2f> imagePoints(6);
cv::Vec3f point_1 = this->getDepth(depth,
objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0); 246,665,
objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0); float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0); 1.0f/depthConstant, 1.0f/depthConstant);
objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0); 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)); // QPointF pt = iter->map(QPointF(0, 0));
imagePoints[0] = cv::Point2f(246,665); imagePoints[0] = cv::Point2f(246,665);
@ -107,8 +132,10 @@ void FindObjectROS::estimate_pose()
imagePoints[2] = cv::Point2f(618,111); imagePoints[2] = cv::Point2f(618,111);
// pt = iter->map(QPointF(0, objectHeight)); // pt = iter->map(QPointF(0, objectHeight));
imagePoints[3] = cv::Point2f(246, 111); 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); cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
cameraMatrix.at<double>(0,0) = 1.0f/depthConstant; cameraMatrix.at<double>(0,0) = 1.0f/depthConstant;
cameraMatrix.at<double>(1,1) = 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_x = cx; //cameraInfo.K.at(2)
float center_y = cy; //cameraInfo.K.at(5) float center_y = cy; //cameraInfo.K.at(5)
std::cout << "cx: " << center_x << " cy: " << center_y << std::endl; // std::cout << "cx: " << center_x << " cy: " << center_y << std::endl;
std::cout << "fx: " << fx << " fy: " << fy << std::endl; // std::cout << "fx: " << fx << " fy: " << fy << std::endl;
bool isInMM = depthImage.type() == CV_16UC1; // is in mm? bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) // 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; bool isValid;
if(isInMM) 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); depth = (float)depthImage.at<uint16_t>(y,x);
isValid = depth != 0.0f; isValid = depth != 0.0f;
} }
@ -198,13 +225,14 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
depth = depthImage.at<float>(y,x); depth = depthImage.at<float>(y,x);
isValid = std::isfinite(depth) && depth > 0.0f; 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 // Check for invalid measurements
if (!isValid) if (!isValid)
{ {
pt.val[0] = pt.val[1] = pt.val[2] = bad_point; 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 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[0] = (float(x) - center_x) * depth * constant_x;
pt.val[1] = (float(y) - center_y) * depth * constant_y; pt.val[1] = (float(y) - center_y) * depth * constant_y;
pt.val[2] = depth*unit_scaling; 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; return pt;
} }