I'm trying to measure a cube without depth. I have the following information
A plane that is position at 0,0,0 with normal up (0,1,0) Detected corners of the cube, I choose them using the click of the points. I convert the points into 3D using the inverse of camera matrix at Z = 1 Then I do line plane intersection to get 3D Positions of the clicked points. My problems 1. How do I correctly visualize the results, right now I'm trying to visualize a plane, but it's always centered at camera position 2. Is the process correct ? Is there are any other steps that I need to do ? 4. Giving camera position and orientation, How is that useful ?
Here is the picture progress so far
//Generate a camera intrinsic matrix
cv::Matx33d K;
K<< 1700, 0, src.cols / 2,
0, 1700, src.rows / 2,
0, 0, 1;
std::vector<cv::Vec3d> pointsTransformed3D;
Mat Pose = (Mat_<double>(3, 4)
<< 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
Mat H = (Mat_<double>(3,3));
Mat transposePose = (Mat_<double>(3, 3));
//transpose(Pose, transposePose);
//H = K*transposePose;
std::vector<cv::Vec3d> points3D;
for (int i = 0; i < corners.size(); i++)
{
cv::Vec3d pt;
pt[2] = 1.0f;
pt[0] = (corners[i].x);
pt[1] = (corners[i].y);
points3D.push_back(pt);
}
cv::transform(points3D, pointsTransformed3D, Pose);
std::vector<cv::Vec3d> points3DProjected;
cv::transform(pointsTransformed3D, points3DProjected, K.inv());
for (int i = 0; i < points3DProjected.size(); i++)
{
points3DProjected[i][0] = points3DProjected[i][0] / points3DProjected[i][2];
points3DProjected[i][1] = points3DProjected[i][1] / points3DProjected[i][2];
points3DProjected[i][2] = points3DProjected[i][2] / points3DProjected[i][2];
}
Mat rot = (Mat_<double>(3, 3)
<< 1, 0, 0,
0, 1, 0,
0, 0, 1);
Mat translVec(3, 1, CV_64F);
translVec.at<double>(0, 0) = 0;
translVec.at<double>(1, 0) = 0;
translVec.at<double>(2, 0) = 0;
std::vector<Ray> rays;
for (int i = 0; i < points3DProjected.size(); i++)
{
Ray ray;
ray.origin = Vec3f(translVec.at<double>(0, 0), translVec.at<double>(1, 0),
translVec.at<double>(2, 0));
ray.direction = Vec3f(points3DProjected[i][0] - translVec.at<double>(0, 0),
points3DProjected[i][1] - translVec.at<double>(1, 0), points3DProjected[i][2] - translVec.at<double>(2, 0));
ray.direction = normalize(ray.direction);
rays.push_back(ray);
}
std::vector<cv::Vec3f> contacts;
for (int i = 0; i < points3DProjected.size(); i++)
{
Vec3f pt(0, 0, 0);
cv::Vec3f contact;
std::pair<bool, double> test = linePlaneIntersection(rays[i].direction, rays[i].origin, Vec3f(0, 1, 0), pt);
if (test.first == true)
{
cv::Vec3f contact(rays[i].origin + (rays[i].direction) * test.second);
contacts.push_back(contact);
}
}
//Project the 3D Point onto 2D plane
std::vector<cv::Point2d> points_2d;
std::vector<cv::Point3d> points_3d;
std::vector<cv::Point3d> lines_3d;
std::vector<cv::Point2d> lines_2d;
for (int i = 0; i < points3DProjected.size(); i++)
{
cv::Point3d pt;
pt.x = (points3DProjected[i][0]);
pt.y = (points3DProjected[i][1]);
pt.z = (points3DProjected[i][2]);
points_3d.push_back(pt);
}
// Create zero distortion
cv::Mat distCoeffs(4, 1, cv::DataType<double>::type);
distCoeffs.at<double>(0) = 0;
distCoeffs.at<double>(1) = 0;
distCoeffs.at<double>(2) = 0;
distCoeffs.at<double>(3) = 0;
cv::Mat rvecR(1, 3, cv::DataType<double>::type);//rodrigues rotation matrix
cv::Rodrigues(rot, rvecR);
cv::projectPoints(points_3d, rot, translVec, K, distCoeffs, points_2d);
for (size_t i = 0; i < points_2d.size(); i++)
{
cv::Point2d pt;
pt.x = points_2d[i].x;
pt.y = points_2d[i].y;
cv::circle(src, pt, 10, cv::Scalar(255, 0, 255), -1);
}
Mat pCloud(points3DProjected.size(), points3DProjected.size(), CV_32FC3);
for (int y = 0; y < points3DProjected.size(); y++) {
float X = (points3DProjected[y][0] - src.cols/2) * 1 / 1700;
float Y = (points3DProjected[y][1] - src.rows/2) * 1 / 1700;
pCloud.at<Vec3f>(y, y) = Vec3f(X, Y, 1);
}
viz::Viz3d myWindow("Point Cloud");
myWindow.showWidget("Coordinate Widget", viz::WCoordinateSystem());
viz::WCloud wcloud(pCloud, viz::Color::red());
vector<viz::WLine> wlines;
for (int i = 0; i < outputlines.size(); i++)
{
cv::Point3d pt1;
pt1.x = outputlines[i][0];
pt1.y = outputlines[i][1];
pt1.z = outputlines[i][2];
cv::Point3d pt2;
pt2.x = outputlines[i][3];
pt2.y = outputlines[i][4];
pt2.z = outputlines[i][5];
viz::WLine wLine(pt1, pt2);
wlines.push_back(wLine);
}
for (int i = 0; i < wlines.size(); i++)
{
myWindow.showWidget("CLOUD", wlines[i]);
}
cv::viz::WCameraPosition camera(K, src, 30.0);
myWindow.showWidget("plane2", cv::viz::WPlane(Vec3d(0, 0, 0), Vec3d(0.0, 1.0, 1.0), Vec3d(0, 1.0, 0.0), Size2d(1.0, 0.5), cv::viz::Color::gold()));
myWindow.showWidget("CLOUD", camera);
myWindow.spin();
}
}
cv:waitKey(0);
return 0;
}