How does this code work?

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?

---

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.

---

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!

---

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.