anti - 1 month ago 11

C++ Question

I am detecting a printed Aruco marker using opencv 3.2:

`aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs,tvecs);`

this returns a translation and rotation vector for the marker. What I need, is the 3d coordinates for each corner of the marker.

As i know the marker length, i could do something like

`corner1 = tvecs[0] - markerlength /2;`

corner2 = tvecs[0] + markerlength /2;

....

But is there an better way? Or an existing function?

To sum up, I have:

a 3d point in the center of a 2d square.

the length of the sides of that square.

the rotation value of the square.

How can I find the 3d coordinates of the corners?

Answer Source

First, let's assume that we only have one marker given with `side = 2 * half_side`

.

Second, `aruco::detectMarker`

returns the relative position of the camera in the marker's world. Thus, I assume that you are looking for the coordinates of the corners ** in camera's world**.

Then, in marker's space:

```
[ half_side ] [ 0 ]
E = [ 0 ], F = [ half_side ]
[ 0 ] [ 0 ]
```

where the center `O`

of the square has coordinate `tvec`

(in camera's world) and rotation mat of the marker `rot_mat`

is computed by `cv::Rodrigues(rvec,rot_mat)`

.

Now, using the pinhole camera model, the relation between coordinates of a point `P`

in cam's world and marker's world is:

```
[P_x_cam] [P_x_marker]
[P_y_cam] = rot_mat * [P_y_marker] + tvec
[P_z_cam] [P_z_marker]
```

for example, the center `O`

, which is `[0,0,0]`

in marker's world, is `tvec`

in cam's world.

So, the coordinates of `E`

in cam's world are:

```
[E_x_cam] [half_side]
|E_y_cam| = rot_mat * | 0 | + tvec
[E_z_cam] [ 0 ]
```

Magically, it is the sum of `rot_mat`

's first column multiplied by `half_size`

and `tvec`

. Similarly,
the coodinates of `F`

is `rot_mat`

's second column multiplied by `half_size`

and `tvec`

.

Now, the corners can be computed, for example

```
C - O = (E - O) + (F - O), B - O = (E - O) - (F - O)
```

where `E-O`

is exactly `rot_mat`

's first column multiplied by `half_size`

.

With all that in mind, we can compose the function:

```
vector<Point3f> getCornersInCameraWorld(double side, Vec3d rvec, Vec3d tvec){
double half_side = side/2;
// compute rot_mat
Mat rot_mat;
Rodrigues(rvec, rot_mat);
// transpose of rot_mat for easy columns extraction
Mat rot_mat_t = rot_mat.t();
// the two E-O and F-O vectors
double * tmp = rot_mat_t.ptr<double>(0);
Point3f camWorldE(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);
tmp = rot_mat_t.ptr<double>(1);
Point3f camWorldF(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);
// convert tvec to point
Point3f tvec_3f(tvec[0], tvec[1], tvec[2]);
// return vector:
vector<Point3f> ret(4,tvec_3f);
ret[0] += camWorldE + camWorldF;
ret[1] += -camWorldE + camWorldF;
ret[2] += -camWorldE - camWorldF;
ret[3] += camWorldE - camWorldF;
return ret;
}
```

Note 1: I hate that SO doesn't have MathJax

Note 2: there must be some faster implementation which I don't know of.