Comment fonctionne ce code?

Question

J'ai trouvé ce morceau de code C ++ sur un forum que je ne peux pas bien comprendre. Comme je n'ai pas leur bibliothèque qui effectue matrice / math vectoriel, je dois comprendre manuellement et à reproduire la fonctionnalité.

Calculer les angles de rotation Euler entre 2 vecteurs .. nous utilisons la formule Rodrigues

    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 >>;

À partir d'ici les choses se compliquent:

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

vous ajoutez toutes les propriétés de la matrice eye3?
vous multipliez avec toutes les propriétés de la matrice de axis_skewed?
et ce qui est R? un vecteur ou une matrice? ou le nombre?

est simple.

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

Encore une fois, cela est délicat:

    // $result is the resulting vector

    $result = ($R * $vectorMatr);

vous multipliez le vecteur avec la matrice pour obtenir le vecteur résultant à l'aide de matrice standard multiplicatif?
vous multipliez les deux de la matrice, puis transformer le point en utilisant la matrice?

Answer 1

Je suis sûr que ce psuedocode. Il est certainement pas C ++. Toutes les fonctions sont assez explicites.

acos () --- explicite

$ V1 * $ V2 --- produit scalaire ( Note: , qui devrait normalement être interprété comme une multiplication de matrice régulière, mais, mais dans le contexte de "flotter $ angle = ACOS ($ V1 * V2 $)", il n'a pas de sens comme autre chose qu'un produit scalaire)

croix () --- produit croisé

normalizeVector () --- explicite

sin (angle de $) * axis_skewed de $ --- ceci est une multiplication scalaire

l'obtenir?

EDIT

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

$ eye3 - est une matrice 3x3

sin (angle de $) * axis_skewed de $ --- ceci est une multiplication scalaire, ce qui entraîne une autre matrice 3x3

(1-cos (angle de $)) * axis_skewed de $ --- ceci est une multiplication scalaire, ce qui entraîne une autre matrice 3x3

(précédent) * axis_skewed de $ --- ceci est une multiplication de matrice régulière, ce qui entraîne dans une autre matrice 3x3

Cela nous laisse avec:

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

Ce qui est juste plus régulière de la matrice entrywise.

Answer 2

D'après ce que je peux dire la dernière partie est une multiplication de matrice stanadard. A [3x3] une fois [3x1] donnera un [3x1]. Je n'aime pas la syntaxe ne est pas facile à lire ...

Modifier

$ R est un [3x3] matrice comme porcherie a montré, R = [3x3] + sin (scalaire) [3x3] + (1-cos (scalaire)) [3x3] * [ 3x3].

Le second terme est un [3x3] à chaque élément mis à l'échelle par sin (angle), le troisième terme est une multiplication de la matrice d'un [3x3] * [3x3], résultant en un autre [3x3].

Ce troisième élément est également mis à l'échelle par le facteur (1-cos (angle)).

= S [3x3] élément R est effectuée résultante sage (ie si j'ai R [3x3] + T [3x3], R [1,1] = S [1,1] + T [1,1 alors R] [1,2] = S [1,2] + T [1,2] .... etc.

---

Si vous cherchez à faire quelque chose de semblable à cet exemple il suffit d'utiliser Matlab -. La syntaxe que vous avez envoyé à confusion et ne pas lire facilement

Sur une note côté escouades nécessitent moins les opérations à effectuer une rotation 3D que les angles d'Euler (et ne fonctionne pas sur les questions autour de pi / 2), donc si vous avez un jour deux passer le temps à lire sur eux. Il n'y a pas trop derrière le calcul soit, afin de lui donner un coup de feu!

Answer 3

Vous essayez de, pour lequel Rodrigues est une forme raccourcie faire l'exponentielle de la matrice de axis_skewed $ [3] [3].

Je vous suggère d'utiliser simplement le cv de OpenCV :: fonction Rodrigues si vous mettez cela en C ++ ...

---

cv :: Mat axis_skewed;

..... // mettre les valeurs dans axis_skewed

cv :: Mat R; // sera 3x3 lorsque vous avez terminé

cv :: Rodgrigues (axis_skewed, R)

---

fait ...

// voici Rodrigues  R $ = $ eye3 + sin (angle de $) * axis_skewed $ + (1-cos (angle de $)) * $ * axis_skewed axis_skewed de $;

Ceci est juste un raccourci pour: R = exponential_of_matrix (axis_skewed)

par exemple. Matlab vous utiliseriez EXPM (axis_skewed). Il y a juste une formule analytique pour écrire la réponse; Sinon, vous pouvez faire R = I + axis_skewed + axis_skewed / 2 + ... + axis_skewed ^ N / (N factoriel) pour un tas de termes et d'obtenir la même réponse.

Alors wikipedia cours se développe sur les mathématiques un peu plus que ci-dessus: http: // fr. wikipedia.org/wiki/Rodrigues%27_rotation_formula

La version OpenCV de votre code ci-dessus, en C ++ / C, 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];

Je vous suggère svn checkout OpenCV, construire, puis faire un test pour vous de vérifier cv :: Rodrigues vous donne la même réponse que votre autre code, puis le port de la fonction à votre projet C ++. Il serait encore plus facile de relier juste OpenCV, mais peut-être que vous ne voulez pas faire cela.