Поиск перевода и масштабирования по двум наборам точек, чтобы получить наименьшую квадратную ошибку на их расстоянии?

Ответ 1

Поскольку вы уже используете алгоритм Kabsch, просто посмотрите на бумагу Umeyama, которая расширяет ее, чтобы получить масштаб. Все, что вам нужно сделать, это получить стандартное отклонение ваших очков и рассчитать масштаб как:

(1/sigma^2)*trace(D*S)

где D - диагональная матрица в разложении SVD в оценке вращения, а S - либо единичная матрица, либо диагональная матрица [1 1 -1], в зависимости от знака детерминанта UV (который Кабш использует для коррекции отражений в правильные вращения). Поэтому, если у вас есть [2000, 200, 20], умножьте последний элемент на + -1 (в зависимости от знака детерминанта UV), суммируйте их и разделите на стандартное отклонение ваших точек, чтобы получить масштаб.

Вы можете переработать следующий код, который использует библиотеку Eigen:

typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3d_U; // microsoft 32-bit compiler can't put Eigen::Vector3d inside a std::vector. for other compilers or for 64-bit, feel free to replace this by Eigen::Vector3d

/**
 *  @brief rigidly aligns two sets of poses
 *
 *  This calculates such a relative pose <tt>R, t</tt>, such that:
 *
 *  @code
 *  _TyVector v_pose = R * r_vertices[i] + t;
 *  double f_error = (r_tar_vertices[i] - v_pose).squaredNorm();
 *  @endcode
 *
 *  The sum of squared errors in <tt>f_error</tt> for each <tt>i</tt> is minimized.
 *
 *  @param[in] r_vertices is a set of vertices to be aligned
 *  @param[in] r_tar_vertices is a set of vertices to align to
 *
 *  @return Returns a relative pose that rigidly aligns the two given sets of poses.
 *
 *  @note This requires the two sets of poses to have the corresponding vertices stored under the same index.
 */
static std::pair<Eigen::Matrix3d, Eigen::Vector3d> t_Align_Points(
    const std::vector<Vector3d_U> &r_vertices, const std::vector<Vector3d_U> &r_tar_vertices)
{
    _ASSERTE(r_tar_vertices.size() == r_vertices.size());
    const size_t n = r_vertices.size();

    Eigen::Vector3d v_center_tar3 = Eigen::Vector3d::Zero(), v_center3 = Eigen::Vector3d::Zero();
    for(size_t i = 0; i < n; ++ i) {
        v_center_tar3 += r_tar_vertices[i];
        v_center3 += r_vertices[i];
    }
    v_center_tar3 /= double(n);
    v_center3 /= double(n);
    // calculate centers of positions, potentially extend to 3D

    double f_sd2_tar = 0, f_sd2 = 0; // only one of those is really needed
    Eigen::Matrix3d t_cov = Eigen::Matrix3d::Zero();
    for(size_t i = 0; i < n; ++ i) {
        Eigen::Vector3d v_vert_i_tar = r_tar_vertices[i] - v_center_tar3;
        Eigen::Vector3d v_vert_i = r_vertices[i] - v_center3;
        // get both vertices

        f_sd2 += v_vert_i.squaredNorm();
        f_sd2_tar += v_vert_i_tar.squaredNorm();
        // accumulate squared standard deviation (only one of those is really needed)

        t_cov.noalias() += v_vert_i * v_vert_i_tar.transpose();
        // accumulate covariance
    }
    // calculate the covariance matrix

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(t_cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
    // calculate the SVD

    Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();
    // compute the rotation

    double f_det = R.determinant();
    Eigen::Vector3d e(1, 1, (f_det < 0)? -1 : 1);
    // calculate determinant of V*U^T to disambiguate rotation sign

    if(f_det < 0)
        R.noalias() = svd.matrixV() * e.asDiagonal() * svd.matrixU().transpose();
    // recompute the rotation part if the determinant was negative

    R = Eigen::Quaterniond(R).normalized().toRotationMatrix();
    // renormalize the rotation (not needed but gives slightly more orthogonal transformations)

    double f_scale = svd.singularValues().dot(e) / f_sd2_tar;
    double f_inv_scale = svd.singularValues().dot(e) / f_sd2; // only one of those is needed
    // calculate the scale

    R *= f_inv_scale;
    // apply scale

    Eigen::Vector3d t = v_center_tar3 - (R * v_center3); // R needs to contain scale here, otherwise the translation is wrong
    // want to align center with ground truth

    return std::make_pair(R, t); // or put it in a single 4x4 matrix if you like
}

Ответ 2

Возможно, вы захотите попробовать ICP (Iterative ближайшая точка). Учитывая два набора трехмерных точек, он скажет вам, что преобразование (вращение + перевод) переходит от первого к второму. Если вас интересует облегченная реализация С++, попробуйте libicp.

Удачи!

Ответ 3

Начните с перевода обоих наборов точек. Таким образом, их центроид совпадает с началом системы координат. Перевод вектора - это просто разница между этими центроидами.

Теперь мы имеем два набора координат, представленных в виде матриц P и Q. Один набор точек может быть получен из другого путем применения некоторого линейного оператора (который выполняет как масштабирование, так и вращение). Этот оператор представлен матрицей 3x3 X:

P * X= Q

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

Простой (но, вероятно, не численно стабильный) способ его решения состоит в том, чтобы умножить обе части уравнения на транспонированную матрицу P (чтобы избавиться от неквадратных матриц), а затем умножить оба части уравнения к инвертированному P ^T * P:

P ^T * P * X= P ^T * Q

X= (P ^T * P) ^-1 * P ^T * Q

Применение Разложение сингулярного значения в матрицу X дает две матрицы вращения и матрицу с масштабными коэффициентами:

X= U * S * V

Здесь S - диагональная матрица с масштабными коэффициентами (один масштаб для каждой координаты), U и V - матрицы вращения, поворачивает точки так, что их можно масштабировать вдоль осей координат, а другой вращает их еще раз, чтобы выровнять их ориентацию ко второму набору точек.

Пример (для простоты используются двумерные точки):

P =  1     2     Q =   7.5391    4.3455
     2     3          12.9796    5.8897
    -2     1          -4.5847    5.3159
    -1    -6         -15.9340  -15.5511

После решения уравнения:

X =  3.3417   -1.2573
     2.0987    2.8014

После разложения SVD:

U = -0.7317   -0.6816
    -0.6816    0.7317

S =  4  0
     0  3

V = -0.9689   -0.2474
    -0.2474    0.9689

Здесь SVD правильно восстановил все манипуляции, которые я выполнил на матрице P, чтобы получить матрицу Q: поверните на угол 0,75, масштаб оси X на 4, масштаб оси Y на 3, поверните на угол -0,25.

Если наборы точек масштабируются равномерно (коэффициент масштабирования равен каждой оси), эта процедура может быть значительно упрощена.

Просто используйте алгоритм Кабша, чтобы получить значения перевода/вращения. Затем выполните эти переводы и вращение (центроиды должны совпадать с началом координат). Тогда для каждой пары точек (и для каждой координаты) оцените Линейная регрессия. Коэффициент линейной регрессии - это точно масштабный коэффициент.

Ответ 4

Для трехмерных точек проблема известна как проблема абсолютной ориентации. Реализация С++ доступна из Eigen http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60 и бумаги http://web.stanford.edu/class/cs273/refs/umeyama.pdf

вы можете использовать его через opencv путем преобразования матриц в собственные с помощью вызовов cv:: cv2eigen().

Ответ 5

Хорошее объяснение Поиск оптимального вращения и трансляции между соответствующими 3D-точками

Код находится в matlab, но тривиально преобразовать в opengl с помощью функции cv:: SVD

Ответ 6

Общее преобразование, а также масштаб можно получить с помощью Procrustes Analysis. Он работает, накладывая объекты друг на друга и пытается оценить трансформацию из этой настройки. Он неоднократно использовался в контексте ПМС. На самом деле, ваш предпочтение, алгоритм Кабаша, является частным случаем этого.

Кроме того, алгоритм выравнивания Хорна (основанный на кватернионах) также находит очень хорошее решение, будучи достаточно эффективным. Также доступна реализация Matlab.

Ответ 7

Масштабирование может быть выведено без SVD, если ваши точки равномерно масштабированы во всех направлениях (я тоже не мог понять масштабную матрицу SVD). Вот как я решил ту же проблему:

• Измерьте расстояния каждой точки до других точек облака точек, чтобы получить 2d таблицу расстояний, где вход в (i, j) является нормой (point_i-point_j). Сделайте то же самое для другого облака точек, так что вы получите две таблицы: одну для оригинальной, а другую для восстановленных точек.

• Разделите все значения в одной таблице на соответствующие значения в другой таблице. Поскольку точки соответствуют друг другу, расстояния тоже. В идеальном случае итоговая таблица имеет все значения, равные друг другу, и это масштаб.

• Среднее значение разделов должно быть близко к масштабу, который вы ищете. Среднее значение также близко, но я выбрал медианный, чтобы исключить выбросы.

Теперь вы можете использовать значение шкалы для масштабирования всех восстановленных точек, а затем перейти к оценке вращения.

Совет. Если в облаках точек слишком много точек для нахождения расстояний между ними, то и меньшая часть расстояний будет работать, если она является тем же подмножеством для обоих облаков. В идеале, только одна пара расстояний будет работать, если нет шумов измерения, например, когда одно облако точек непосредственно получается из другого, просто вращая его.

Ответ 8

вы также можете использовать ScaleRatio ICP, предложенную BaoweiLin Код можно найти в github