# Aruco markers with openCv, get the 3d corner coordinates?

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.