Calculating distance between 2 homography planes that have shared ground plane - opencv

Question

I think the easiest is to explain problem with image:
I have two cubes (same size) that are laying on the table. One of their side is marked with green color (for easy tracking). I want to calculate the relative position (x,y) of left cube to the right cube (red line on the picture) in cube size unit.
Is it even possible? I know problem would be simple if those two green sides would have common plane - like top side of cube however I can't use that for tracking. I would just calculate homography for one square and multiply with other cube corner.
Should I 'rotate' homography matrix by multiplying with 90deegre rotation matrix to get 'ground' homography? I plan to do processing in smartphone scenario so maybe gyroscope, camera intrinsic params can be of any value.

Answer 1

This is possible.
Let's assume (or state) that the table is the z=0-plane and that your first box is at the origin of this plane. This means that green corners of the left box have the (table-)coordinates (0,0,0),(1,0,0),(0,0,1) and (1,0,1). (Your box has the size 1).
You also have the pixel coordinates of these points. If you give these 2d and 3d-values (as well as the intrinsics and distortion of the camera) to cv::solvePnP, you get the relative Pose of the camera to your box (and the plane).
In the next step, you have to intersect the table-plane with the ray that goes from your camera's center through the lower right corner pixels of the second green box. This intersection will look like (x,y,0) and [x-1,y] will be translation between the right corners of your boxes.

Answer 2

If you have all the information (camera intrinsics) you can do it the way FooBar answered.
But you can use the information that the points lie on a plane even more directly with a homography (no need to calculate rays etc):
Compute the homography between the image plane and the ground plane.
Unfortunately you need 4 point correspondences, but there are only 3 cube-points visible in the image, touching the ground plane.
Instead you can use the top-plane of the cubes, where the same distance can be measured.
first the code:
int main()
{
// calibrate plane distance for boxes
cv::Mat input = cv::imread("../inputData/BoxPlane.jpg");
// if we had 4 known points on the ground plane, we could use the ground plane but here we instead use the top plane
// points on real world plane: height = 1: // so it's not measured on the ground plane but on the "top plane" of the cube
std::vector<cv::Point2f> objectPoints;
objectPoints.push_back(cv::Point2f(0,0)); // top front
objectPoints.push_back(cv::Point2f(1,0)); // top right
objectPoints.push_back(cv::Point2f(0,1)); // top left
objectPoints.push_back(cv::Point2f(1,1)); // top back
// image points:
std::vector<cv::Point2f> imagePoints;
imagePoints.push_back(cv::Point2f(141,302));// top front
imagePoints.push_back(cv::Point2f(334,232));// top right
imagePoints.push_back(cv::Point2f(42,231)); // top left
imagePoints.push_back(cv::Point2f(223,177));// top back
cv::Point2f pointToMeasureInImage(741,200); // bottom right of second box
// for transform we need the point(s) to be in a vector
std::vector<cv::Point2f> sourcePoints;
sourcePoints.push_back(pointToMeasureInImage);
//sourcePoints.push_back(pointToMeasureInImage);
sourcePoints.push_back(cv::Point2f(718,141));
sourcePoints.push_back(imagePoints[0]);
// list with points that correspond to sourcePoints. This is not needed but used to create some ouput
std::vector<int> distMeasureIndices;
distMeasureIndices.push_back(1);
//distMeasureIndices.push_back(0);
distMeasureIndices.push_back(3);
distMeasureIndices.push_back(2);
// draw points for visualization
for(unsigned int i=0; i<imagePoints.size(); ++i)
{
cv::circle(input, imagePoints[i], 5, cv::Scalar(0,255,255));
}
//cv::circle(input, pointToMeasureInImage, 5, cv::Scalar(0,255,255));
//cv::line(input, imagePoints[1], pointToMeasureInImage, cv::Scalar(0,255,255), 2);
// compute the relation between the image plane and the real world top plane of the cubes
cv::Mat homography = cv::findHomography(imagePoints, objectPoints);
std::vector<cv::Point2f> destinationPoints;
cv::perspectiveTransform(sourcePoints, destinationPoints, homography);
// compute the distance between some defined points (here I use the input points but could be something else)
for(unsigned int i=0; i<sourcePoints.size(); ++i)
{
std::cout << "distance: " << cv::norm(destinationPoints[i] - objectPoints[distMeasureIndices[i]]) << std::endl;
cv::circle(input, sourcePoints[i], 5, cv::Scalar(0,255,255));
// draw the line which was measured
cv::line(input, imagePoints[distMeasureIndices[i]], sourcePoints[i], cv::Scalar(0,255,255), 2);
}
// just for fun, measure distances on the 2nd box:
float distOn2ndBox = cv::norm(destinationPoints[0]-destinationPoints[1]);
std::cout << "distance on 2nd box: " << distOn2ndBox << " which should be near 1.0" << std::endl;
cv::line(input, sourcePoints[0], sourcePoints[1], cv::Scalar(255,0,255), 2);
cv::imshow("input", input);
cv::waitKey(0);
return 0;
}
Here's the output which I want to explain:
distance: 2.04674
distance: 2.82184
distance: 1
distance on 2nd box: 0.882265 which should be near 1.0
those distances are:
1. the yellow bottom one from one box to the other
2. the yellow top one
3. the yellow one on the first box
4. the pink one
so the red line (you asked for) should have a length of nearly exactly 2 x cube side length. But we have some error as you can see.
the better/more correct your pixel positions are before homography computation, the more exact your results.
You need a pinhole camera model, so undistort your camera (in real world application).
keep in mind too, that you could compute the distances on the ground plane, if you had 4 linear points visible there (that dont lie on same line)!