Eigen - Реортогонализация матрицы вращения

Question

После умножения большого количества матриц вращения конечный результат может перестать быть действительной матрицей вращения из-за проблем с округлением (деортогонализация)

Один из способов повторной ортогонализации состоит в следующем:

1. Преобразовать матрицу вращения в представление оси-угла ( ссылка)
2. Преобразовать обратно ось-угол в матрицу вращения ( ссылка)

Есть ли что-то в библиотеке Eigen, которая делает то же самое, скрывая все детали? Или есть лучший рецепт?

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

Answer 1

Я не использую Eigen и не удосужился посмотреть API, но здесь есть простая, вычислительно дешевая и стабильная процедура для повторной ортогонализации матрицы вращения. Эта процедура ортогонализации взята из Направления косинусной матрицы IMU: теория Уильяма Премерлани и Пола Бизера; уравнения 19-21.

Позволять x, y а также z быть векторными строками (слегка испорченной) матрицы вращения. Позволять error=dot(x,y) где dot() это скалярное произведение. Если матрица была ортогональной, то скалярное произведение x а также y, это error будет ноль.

error распространяется по x а также y в равной степени: x_ort=x-(error/2)*y а также y_ort=y-(error/2)*x, Третий ряд z_ort=cross(x_ort, y_ort), который по определению ортогонален x_ort а также y_ort,

Теперь вам нужно нормализовать x_ort, y_ort а также z_ort так как эти векторы должны быть единичными векторами.

x_new = 1/2*(3-dot(x_ort,x_ort))*x_ort
y_new = 1/2*(3-dot(y_ort,y_ort))*y_ort
z_new = 1/2*(3-dot(z_ort,z_ort))*z_ort

Вот и все, были сделаны.

Это должно быть довольно легко реализовать с помощью API, предоставляемого Eigen. Вы можете легко придумать другие ортогональные процедуры, но я не думаю, что это будет заметно на практике. Я использовал описанную выше процедуру в своем приложении отслеживания движения, и оно работало прекрасно; это и стабильно и быстро.

Answer 2

Вы можете использовать QR-декомпозицию для систематической повторной ортогонализации, где вы заменяете исходную матрицу на Q-фактор. В подпрограммах библиотеки вы должны проверить и исправить, если необходимо, путем отрицания соответствующего столбца в Q, что диагональные элементы R положительны (близко к 1, если исходная матрица была близка к ортогональной).

Ближайшая матрица вращения Q к данной матрице получается из полярного или QP-разложения, где P - положительная полуопределенная симметричная матрица. Разложение QP может быть вычислено итеративно или с использованием SVD. Если последний имеет факторизацию USV', то Q=UV'.

Answer 3

Разложение по единственному значению должно быть очень устойчивым. Цитировать из ссылки:

Пусть M=UΣV - сингулярное разложение M, тогда R=UV.

Для вашей матрицы сингулярные значения в Σ должны быть очень близки к единице. Матрица R гарантированно будет ортогональной, что является определяющим свойством матрицы вращения. Если при вычислении исходной матрицы вращения не было ошибок округления, то значение R будет точно таким же, как у вашего M, с точностью до числовой точности.

Answer 4

— матрица, которую мы хотим ортонормировать, и — матрица вращения, ближайшая к .

Аналитическое решение

      Matrix R = M*inverse(sqrt(transpose(M)*M));

Итеративное решение

      // To re-orthogonalize matrix M, repeat:
M = 0.5f*(inverse(transpose(M)) + M);
// until M converges

сходится к ближайшей матрице вращения. Количество цифр точности будет примерно удваиваться с каждой итерацией.

Проверить, является ли сумма квадратов элементов(M - M^-T)/2меньше квадрата вашей цели по ошибке, чтобы узнать, когда он соответствует вашему порогу точности. является обратным транспонированием .

Почему это работает

Мы хотим найти матрицу вращения, ближайшую к . Определим ошибку как сумму квадратов разностей элементов. то есть свести к минимумуtrace((M - R)^T (M - R)).

Аналитическое решениеR = M (M^T M)^-(1/2), изложено здесь.

Проблема в том, что для этого нужно найти квадратный корень изM^T M. Однако, если мы заметим, есть много матриц, ближайшая матрица вращения которых равна . Один из нихM (M^T M)^-1, что упрощается до , обратное транспонирование. Приятно то, что иM^-Tнаходятся на противоположных сторонах , (интуитивно какaи1/aнаходятся на противоположной стороне1).

Мы признаем, что средний,(M + M^-T)/2будет ближе к , и поскольку это линейная комбинация, она также будет поддерживаться как ближайшая матрица вращения. Делая это рекурсивно, мы сходимся кR.

Сходимость в наихудшем случае (спекулятивная)

Поскольку он связан с вавилонским алгоритмом квадратного корня, он сходится квадратично.

Точная ошибка наихудшего случая после одной итерации матрицыMи ошибкаeявляетсяnextE:

      nextE = e^2/(2 e + 2)
e = sqrt(trace((M - R)^T (M - R)))
R = M (M^T M)^-(1/2)

Answer 5

В это время:

#include <Eigen/Geometry>

Eigen::Matrix3d mmm;
Eigen::Matrix3d rrr;
                rrr <<  0.882966, -0.321461,  0.342102,
                        0.431433,  0.842929, -0.321461,
                       -0.185031,  0.431433,  0.882966;
                     // replace this with any rotation matrix

mmm = rrr;

Eigen::AngleAxisd aa(rrr);    // RotationMatrix to AxisAngle
rrr = aa.toRotationMatrix();  // AxisAngle      to RotationMatrix

std::cout <<     mmm << std::endl << std::endl;
std::cout << rrr     << std::endl << std::endl;
std::cout << rrr-mmm << std::endl << std::endl;

Что является хорошей новостью, потому что я могу избавиться от своего собственного метода и иметь одну головную боль меньше (как можно быть уверенным, что он заботится обо всех особенностях?),

но я очень хочу ваше мнение о лучших / альтернативных способах:)

Answer 6

Альтернативой является использование Eigen::Quaternion представлять ваше вращение. Это гораздо проще нормализовать, и rotation*rotation продукты, как правило, быстрее. Если у вас много rotation*vector продукты (с той же матрицей), вы должны локально преобразовать кватернион в матрицу 3х3.

Answer 7

Вот реализация с использованием Eigen. Это минимально повлияет на вращение, представленное матрицей вращения, путем корректировки любых двух векторов, которые не являются одинаково ортогональными, по направлению друг к другу или от него.

      void orthonormalize(Eigen::Matrix3d &rot) {
  Vector3d x = rot.col(0);
  Vector3d y = rot.col(1);
  Vector3d z = rot.col(2);

  // normalize
  x.normalize();
  y.normalize();
  z.normalize();

  // orthogonalize
  double errorXY = 0.5 * x.dot(y);
  double errorYZ = 0.5 * y.dot(z);
  double errorZX = 0.5 * z.dot(x);
  rot.col(0) = x - errorXY * y - errorZX * z;
  rot.col(1) = y - errorXY * x - errorYZ * z;
  rot.col(2) = z - errorYZ * y - errorZX * x;
}

Мы можем проверить, что это действительно работает с

      double orthonormalityError(const Matrix3d &rot) {
  Matrix3d prod = rot * rot.transpose();
  return (prod - Matrix3d::Identity()).norm();
}

Matrix3d randomRotationMatrix() {
  Matrix3d rand_mat = Matrix3d::Random();
  HouseholderQR<Matrix3d> qr(rand_mat);
  return rot = qr.householderQ();
}

 void testOrthonormalize() {
  Eigen::Matrix3d rot = randomRotationMatrix();
  rot += Eigen::Matrix3d::Random() * 0.1;
  std::cout << orthonormalityError(rot) << std::endl;
  for (int j = 0; j < 4; j++) {
    orthonormalize(rot);
    std::cout << orthonormalityError(rot) << std::endl;
  }
}

int main(int argc, char** argv) {
  testOrthonormalize();
  return 0;
}

Что печатает:

      0.246795
0.00948343
1.45409e-05
3.59979e-11
3.4066e-16