Trouver la traduction et de l'échelle sur deux ensembles de points pour obtenir des moindres carrés de l'erreur dans leur distance?

Puisque vous êtes déjà à l'aide de Kabsch algorithme, il suffit de jeter un coup d'oeil à Umeyama du papier qui s'étend à elle pour obtenir de l'échelle. Tout ce que vous devez faire est d'obtenir l'écart-type de vos points et de calculer l'échelle:

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

où D est la diagonale de la matrice de décomposition SVD dans la rotation de l'estimation et de S est soit la matrice d'identité ou [1 1 -1] matrice diagonale, selon le signe du déterminant de la UV (Kabsch utilise pour corriger les reflets dans les rotations). Donc, si vous avez [2000, 200, 20], multipliez le dernier élément de +-1 (selon le signe du déterminant de la UV), de la somme et diviser par l'écart-type de vos points pour obtenir de l'échelle.

Vous pouvez recycler le code suivant, qui est à l'aide de la Eigen bibliothèque:

typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3d_U; //microsoft's 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
}

OriginalL'auteur the swine

Commencer avec la traduction des deux ensembles de points. De sorte que leur centre de gravité coïncide avec l'origine du système de coordonnées. Le vecteur de Translation est juste la différence entre ces centroïdes.

Maintenant, nous avons deux ensembles de coordonnées représentés comme des matrices P et Q. Un ensemble de points qui peuvent être obtenus à partir de l'autre par l'application d'un opérateur linéaire (qui effectue à la fois l'échelle et la rotation). Cet opérateur est représenté par une matrice de 3x3 X:

P * X = Q

Trouver la bonne échelle/rotation nous avons juste besoin de résoudre cette équation matricielle, trouver X, puis de le décomposer en plusieurs matrices, chacune représentant certains d'échelle ou de rotation.

Un simple (mais probablement pas numériquement stable) façon de le résoudre est de multiplier les deux parties de l'équation de la matrice transposée P (pour se débarrasser de non-matrices carrées), puis multiplier les deux parties de l'équation à l'inverse de la P^T * P:

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

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

L'application de Décomposition en valeurs singulières à matrice X donne deux matrices de rotation et une matrice avec des facteurs d'échelle:

X = U * S * V

Ici S est une matrice diagonale avec des facteurs d'échelle (une échelle pour chaque coordonnée), U et V sont des matrices de rotation, on tourne correctement les points de sorte qu'ils peuvent être mis à l'échelle le long des axes de coordonnées, l'autre tourne une fois de plus à aligner leur orientation par rapport au deuxième ensemble de points.

---

Exemple (2D points sont utilisés pour des raisons de simplicité):

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

Après la résolution de l'équation:

X =  3.3417   -1.2573
2.0987    2.8014

Après la décomposition SVD:

U = -0.7317   -0.6816
-0.6816    0.7317
S =  4  0
0  3
V = -0.9689   -0.2474
-0.2474    0.9689

Ici SVD a correctement reconstruite à toutes les manipulations j'ai effectué sur la matrice P pour obtenir la matrice Q: rotation de l'angle de 0,75, à l'échelle de l'axe X par 4, à l'échelle de l'axe Y par 3, rotation de l'angle de -0.25.

---

Si des ensembles de points sont effectués de manière uniforme (le facteur d'échelle est égal par chaque axe), cette procédure peut être considérablement simplifiée.

Suffit d'utiliser Kabsch algorithme pour obtenir la traduction/valeurs de rotation. Puis effectuer ces de translation et de rotation (centroïdes devrait coïncider avec l'origine du système de coordonnées). Ensuite, pour chaque paire de points (et pour chaque coordonnée) estimation La régression linéaire. Linéaire coefficient de régression est exactement le facteur d'échelle.

OriginalL'auteur Evgeny Kluev

Une bonne explication Trouver optimal de rotation et de translation entre les correspondants des points 3D

Le code est dans matlab, mais il est trivial de convertir à opengl en utilisant le cv::SVD fonction

OriginalL'auteur Martin Beckett

Échelle peut être déduit sans SVD, si vos points sont uniformément de façon uniforme dans toutes les directions (je ne pouvais pas faire sens de la SVD-s à l'échelle de la matrice). Voici comment j'ai résolu le même problème:

1. Mesurer des distances de chaque point à d'autres points dans le nuage de points pour obtenir un 2d tableau des distances, où l'entrée (i,j) est la norme(point_i-point_j). Faire la même chose pour l'autre point de nuage, de sorte que vous obtenez deux tables, une pour origine et l'autre pour reconstruits points.

2. Diviser toutes les valeurs dans un tableau par les valeurs correspondantes dans l'autre table. Parce que les points correspondent les uns aux autres, les distances en faire trop. Idéalement, le tableau a toutes les valeurs sont égales les unes aux autres, et c'est à cette échelle.

3. La valeur médiane de la les divisions devraient être assez proche de l'échelle que vous recherchez. La valeur moyenne est proche, mais j'ai choisi médian juste pour exclure les valeurs aberrantes.

Maintenant, vous pouvez utiliser la valeur de l'échelle pour l'échelle de tout le reconstruit points et ensuite de procéder à l'estimation de la rotation.

Astuce: Si il y a trop de points dans le nuage de points de calculer des distances entre chacun d'eux, puis un petit sous-ensemble de distances de travail, trop, aussi longtemps que c'est le même sous-ensemble pour les deux nuages de points. Idéalement, juste une distance paire pourrait fonctionner si il n'y a pas de bruit de mesure, électronique.g quand un nuage de points est directement dérivé de l'autre, juste en tournant.

OriginalL'auteur Rasmus