Converting Math functions from Python to c++

Question

I am attempting to convert some opencv code from python to c++, and am a little lost. The python is:

if 0 < R[1,1] < 1:
    # If it gets here, the pose is flipped.

    # Flip the axes. E.g., Y axis becomes [-y0, -y1, y2].
    R *= np.array([
        [ 1, -1,  1],
        [ 1, -1,  1],
        [-1,  1, -1],
    ])

    # Fixup: rotate along the plane spanned by camera's forward (Z) axis and vector to marker's position
    forward = np.array([0, 0, 1])
    tnorm = T / np.linalg.norm(T)
    axis = np.cross(tnorm, forward)
    angle = -2*math.acos(tnorm @ forward)
    R = cv2.Rodrigues(angle * axis)[0] @ R

So far i have:

cv::Mat R;
if (0 < R.at<double>(1, 1) < 1) {

            // Flip the axes.E.g., Y axis becomes[-y0, -y1, y2].
            float mult[9] = { 1, -1, 1, 1, -1, 1,-1, 1, -1 };

            cv::Mat FlipAxes = cv::Mat(3, 3, CV_32F, mult);

            R *= FlipAxes;

                //# Fixup: rotate along the plane spanned by camera's forward (Z) axis and vector to marker's position
            cv::Vec3d forward(0, 0, 1);
            double tnorm = tvecBest / np.linalg.norm(T)
            axis = np.cross(tnorm, forward)
            angle = -2 * math.acos(tnorm @ forward)
            R = cv2.Rodrigues(angle * axis)[0] * R


        }

I am lost on these lines:

double tnorm = tvecBest / np.linalg.norm(T)
axis = np.cross(tnorm, forward)
angle = -2 * math.acos(tnorm @ forward)

what is the equivalent of np.linalg.norm in c++ opencv?

There isn't a direct one you can use cv::sqrt() and cv::pow() — Martin Beckett, Aug 22 '18 at 15:15
How about using [`norm`](https://docs.opencv.org/2.4/modules/core/doc/operations_on_arrays.html#norm), or just directly doing [`normalize`](https://docs.opencv.org/2.4/modules/core/doc/operations_on_arrays.html#normalize)? — Max Langhof, Aug 22 '18 at 15:20
Thanks, can I do this?: `cv::Vec3d tvecNorm(tvecBest); cv::normalize(tvecNorm, tvecNorm);` — anti, Aug 22 '18 at 15:27
could you explain that last comment please? Do you mean loop through each element of teh Mat and check it? — anti, Aug 22 '18 at 16:00
(0 < R.at(1, 1) < 1) result is ((0 < R.at(1,1)) < 1), note extra parenthesis, I think this is not what you want. (0 < R.at(1, 1) < 1) should be written as the && of two bools: (0 < R.at(1, 1)) && (R.at(1, 1) < 1) — 2785528, Aug 22 '18 at 16:11
Also see [Convert Python program to C/C++ code?](https://stackoverflow.com/q/4650243/608639) — jww, Nov 21 '19 at 13:36

score 3 · Accepted Answer · answered Aug 22 '18 at 15:32

3

the np.linalg.norm(T) is just the L2 norm :

sqrt(T[0]*T[0]+T[1]*T[1]+T[2]*T[2])

np.cross(tnorm, forward) is the cross product:

axis[0] = tnorm[1]*forward[2]-tnorm[2]*forward[1]
axis[1] = -tnorm[0]*forward[2]+tnorm[2]*forward[0]
axis[2] = tnorm[0]*forward[1]-tnorm[1]*forward[0]

answered Aug 22 '18 at 15:32

PilouPili

2,601
2
17
31

Thank you! So that gives me: `cv::Vec3d forward(0, 0, 1); cv::Vec3d T(tvecBest); cv::Vec3d tnorm = sqrt(T[0] * T[0] + T[1] * T[1] + T[2] * T[2]); cv::Vec3d axis; axis[0] = tnorm[1] * forward[2] - tnorm[2] * forward[1]; axis[1] = -tnorm[0] * forward[2] + tnorm[2] * forward[0]; axis[2] = tnorm[0] * forward[1] - tnorm[1] * forward[0];` – anti Aug 22 '18 at 15:36
What is the c++ for : `cv::Vec3d angle = -2 * math.acos(tnorm @ forward)` ? – anti Aug 22 '18 at 15:37
the @ is not a mathematical operator in python. Was it / operator ? – PilouPili Aug 22 '18 at 15:46
it is written as @, strangely. It is from here: `https://github.com/opencv/opencv/issues/8813#issuecomment-390462446` – anti Aug 22 '18 at 15:47
aha, there is actually a matlab implementation also, it looks like it should be: `cv::Vec3d angle = -2 * acos(forward*tnorm); ` However, the acos function does not like cv::Vec3d. Do I need to split it out and get three values? – anti Aug 22 '18 at 15:49
1

from what I read they meant @ to be a dot product : tnorm @ forward -> tnorm[0]*forward [0]+ tnorm[1]*forward [1]+ tnorm[2]*forward [2] – PilouPili Aug 22 '18 at 15:51
1

and angle must be a double : double angle = -2 * acos( tnorm[0]*forward [0]+ tnorm[1]*forward [1]+ tnorm[2]*forward [2]) – PilouPili Aug 22 '18 at 15:53

Converting Math functions from Python to c++

1 Answers1