How does this code work?

Question

I found this piece of C++ code on a forum that I can't fully understand. Since I don't have their library that performs matrix/vector math, I need to manually figure it out and replicate the functionality.

Calculate Euler rotation angles between 2 vectors .. we use Rodrigues formula

    vector $V1 = << my first vector >>;
    vector $V2 = << my second vector >>;


    vector $axis;
    float $angle;

    $angle = acos($V1*$V2);
    $axis = normalizeVector((cross($V1,$V2)));


    matrix $axis_skewed[3][3] = <<
    0, (-$axis.z), ($axis.y) ;
    ($axis.z), 0, (-$axis.x) ;
    (-$axis.y), ($axis.x), 0 >>;

    matrix $eye3[3][3] = <<
    1, 0, 0;
    0, 1, 0;
    0, 0, 1 >>;

From here onwards things get tricky:

    // here's Rodrigues
    $R = $eye3 + sin($angle)*$axis_skewed + (1-cos($angle))*$axis_skewed*$axis_skewed;

do you add all the properties of the eye3 matrix?
do you multiply with all the properties of the axis_skewed matrix?
and what is R? a vector or matrix? or number?

This is simple.

    matrix $vectorMatr[3][1];
    $vectorMatr[0][0] = ($V1.x);
    $vectorMatr[1][0] = ($V1.y);
    $vectorMatr[2][0] = ($V1.z);

Again, this is tricky:

    // $result is the resulting vector

    $result = ($R * $vectorMatr);

do you multiply the vector with the matrix to get the resultant vector using standard matrix multiplying?
do you multiply the two matrix's and then transform the point using the matrix?

Answer 1

I'm pretty sure that's psuedocode. It's definitely not C++. All the functions are pretty self explanatory.

acos() --- self explanatory

$V1 * $V2 --- dot product (note:, that would normally be interpreted as a regular matrix multiplication, but but in the context of "float $angle = acos($V1*$V2);", it doesn't make sense as anything other than a dot product)

cross() --- cross product

normalizeVector() --- self explanatory

sin($angle)*$axis_skewed --- this is a scalar multiply

get it?

EDIT

$R = $eye3 + sin($angle)*$axis_skewed + (1-cos($angle))*$axis_skewed*$axis_skewed;

$eye3 -- is a 3x3 matrix

sin($angle)*$axis_skewed --- this is a scalar multiply, resulting in another 3x3 matrix

(1-cos($angle))*$axis_skewed --- this is a scalar multiply, resulting in another 3x3 matrix

(previous)*$axis_skewed --- this is a regular matrix multiplication, resulting in another 3x3 matrix

That leaves us with:

$R = [3x3 matrix] + [3x3 matrix] + [3x3 matrix]

Which is just regular entrywise matrix addition.

Answer 2

From what I can tell the last part is a stanadard matrix multiplication. A [3x3] times a [3x1] will yield a [3x1]. I don't like the syntax its not easy to read...

Edit:

$R is a [3x3] matrix as pigpen has shown, R= [3x3]+sin(scalar)[3x3]+(1-cos(scalar))[3x3]*[3x3].

The second term is a [3x3] with each element scaled by sin(angle), the third term is a matrix multiplication of a [3x3]*[3x3], resulting in another [3x3].

That third element is also scaled by the factor (1-cos(angle)).

The resultant R is performed element wise (i.e. if I have a R[3x3]=S[3x3]+T[3x3], R[1,1]=S[1,1]+T[1,1] then R[1,2]=S[1,2]+T[1,2].... etc.

---

If you're looking to do something similar to this example just use Matlab - the syntax you posted is confusing and not easily read.

On a side note quaternions require less operations to perform a 3D rotation than Euler angles (and don't run into issues around pi/2), so if you have a couple days spend the time reading up on them. There isn't too much behind the math either, so give it a shot!

Answer 3

You're trying to do the matrix exponential of $axis_skewed[3][3] , for which Rodrigues is a shortened form.

I suggest you just use OpenCV's cv::Rodrigues function if you're putting this in C++...

---

cv::Mat axis_skewed;

..... // put the values into axis_skewed

cv::Mat R; // will be 3x3 when done

cv::Rodgrigues( axis_skewed, R )

---

done...

// here's Rodrigues $R = $eye3 + sin($angle)*$axis_skewed + (1-cos($angle))*$axis_skewed*$axis_skewed;

This is just a shortcut for: R = exponential_of_matrix( axis_skewed )

e.g. in matlab you'd use expm( axis_skewed ). There's just an analytic formula to write down the answer; alternatively you could do R = I + axis_skewed + axis_skewed / 2 + ... + axis_skewed ^ N / (N factorial) for a bunch of terms and get the same answer.

Then of course wikipedia expands on the math a bit more than above: http://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

The OpenCV version of your code above, in C++/C, from https://code.ros.org/svn/opencv/trunk/opencv/modules/calib3d/src/calibration.cpp

const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };

        double c = cos(theta);
        double s = sin(theta);
        double c1 = 1. - c;
        double itheta = theta ? 1./theta : 0.;

        rx *= itheta; ry *= itheta; rz *= itheta;

        double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
        double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
        double R[9];
        CvMat matR = cvMat( 3, 3, CV_64F, R );

        // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
        // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
        for( k = 0; k < 9; k++ )
            R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k];

I suggest you svn checkout OpenCV, build it, then make a test for yourself to verify cv::Rodrigues gives you the same answer as your other code, then port the function to your C++ project. It would be even easier to just link to opencv, but maybe you don't want to do that.