Как работает этот код?

Question

Я нашел этот фрагмент кода C ++ на форуме, который я не могу полностью понять.Так как у меня нет их библиотеки, которая выполняет математику / векторную математику, мне нужно вручную выяснить это и воспроизвести функциональность.

Рассчитать углы поворота Эйлера между 2 векторами ... мы используем формулу Родригеса

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

С этого момента все становится сложнее:

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

Вы добавляете все свойства матрицы eye3?
Умножаете ли вы все свойстваосевая матрица?
а что такое R?вектор или матрица?или число?

Это просто.

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

Опять же, это сложно:

    // $result is the resulting vector

    $result = ($R * $vectorMatr);

умножаете ли вы вектор на матрицу, чтобы получить результирующий вектор?используя стандартное умножение матриц?
умножаете ли вы две матрицы, а затем преобразовываете точку, используя матрицу?

Answer 1

Из того, что я могу сказать, последняя часть - стандартное умножение матриц.[3x3] раз a [3x1] даст [3x1].Мне не нравится синтаксис, его нелегко читать ...

Редактировать:

$ R - матрица [3x3], как показала свинарник, R =[3x3] + sin (скаляр) [3x3] + (1-cos (скаляр)) [3x3] * [3x3].

Второй член представляет собой [3x3] с каждымэлемент масштабируется на sin (угол), третий член представляет собой матричное умножение [3x3] * [3x3], что приводит к другому [3x3].

Этот третий элемент также масштабируется с коэффициентом (1-cos (угол)).

Результирующее R выполняется поэлементно (т.е. если у меня R [3x3] = S [3x3] + T [3x3], R [1,1] = S [1, 1] + T [1,1], затем R [1,2] = S [1,2] + T [1,2] .... и т. Д.

---

Если выпытаясь сделать что-то похожее на этот пример, просто используйте Matlab - синтаксис, который вы разместили, сбивает с толку и не так легко читается.

На боковой ноте кватернионы требуют меньше операций для выполнения трехмерного вращения, чем углы Эйлера (и нестолкнитесь с проблемами, связанными с pi / 2), так что если у вас есть пара дней, потратьте время на их чтение. Также не слишком много математики, так чтопопробуй!

Answer 2

Я уверен, что это psuedocode.Это определенно не C ++.Все функции довольно понятны.

acos () --- говорит само за себя

$ V1 * $ V2 --- точка продукта ( примечание: , что будетобычно интерпретируется как регулярное матричное умножение, но в контексте «float $ angle = acos ($ V1 * $ V2);» это не имеет смысла как что-либо, кроме точечного произведения)

cross () --- кросс-произведение

normalizeVector () --- не требует пояснений

sin ($ angle) * $ axis_skewed --- это скалярное умножение

получить?

РЕДАКТИРОВАТЬ

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

$ eye3 - это матрица 3x3

sin ($ angle) * $ axis_skewed --- это скалярное умножение, в результате чего получается еще одна матрица 3x3

(1-cos ($ angle)) * $ axis_skewed --- это скалярное умножение, приводящее к другой матрице 3x3

(previous) * $ axis_skewed --- это умножение регулярной матрицы,в результате получается еще одна матрица 3х3

Это оставляет нас с:

$ R = [матрица 3x3] + [матрица 3x3] + [матрица 3x3]

, что является просто регулярным добавлением матрицы для входа.

Answer 3

Вы пытаетесь сделать экспоненциальную матрицу $ axis_skewed [3] [3], для которой Родригес является укороченной формой.

Я предлагаю вам просто использовать функцию OpenCV cv :: Rodrigues, если вы помещаете это в C ++ ...

---

cv :: Mat axis_skewed;

..... // помещаем значения в axis_skewed

cv :: Mat R; // будет 3х3, когда закончим

cv :: Rodgrigues (axis_skewed, R)

---

сделано ...

// вот Родригес $ R = $ eye3 + sin ($ angle) * $ axis_skewed + (1-cos ($ angle)) * $ axis_skewed * $ axis_skewed;

Это просто сокращение для: R = exponential_of_matrix (axis_skewed)

например. в Matlab вы бы использовали expm (axis_skewed). Есть просто аналитическая формула, чтобы записать ответ; В качестве альтернативы вы можете сделать R = I + axis_skewed + axis_skewed / 2 + ... + axis_skewed ^ N / (N factorial) для нескольких терминов и получить тот же ответ.

Тогда, конечно, Википедия немного расширит математику: http://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

OpenCV-версия вашего кода выше, на 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];

Я предлагаю вам svn оформить заказ OpenCV, скомпилировать его, затем выполнить тест для себя, чтобы убедиться, что cv :: Rodrigues дает вам тот же ответ, что и ваш другой код, а затем перенести функцию в ваш проект C ++. Было бы еще проще просто ссылаться на opencv, но, возможно, вы не хотите этого делать.