IMP logo
IMP Reference Guide  develop.d247202c1c,2019/10/23
The Integrative Modeling Platform
geometric_alignment.h
Go to the documentation of this file.
1 /**
2  * \file IMP/algebra/geometric_alignment.h
3  * \brief align sets of points.
4  *
5  * Copyright 2007-2019 IMP Inventors. All rights reserved.
6  */
7 
8 #ifndef IMPALGEBRA_GEOMETRIC_ALIGNMENT_H
9 #define IMPALGEBRA_GEOMETRIC_ALIGNMENT_H
10 
11 #include <IMP/algebra/algebra_config.h>
12 #include "Vector3D.h"
13 #include "SphereD.h"
14 #include "Rotation3D.h"
15 #include "Transformation3D.h"
16 #include "Transformation2D.h"
17 #include <IMP/check_macros.h>
18 #include <IMP/log.h>
19 #include <IMP/log_macros.h>
20 
21 IMPALGEBRA_BEGIN_NAMESPACE
22 
23 //! Compute the rigid transform bringing the first point set to the second
24 /** The points are assumed to be corresponding (that is, from[0] is aligned
25  to to[0] etc.). The alignment computed is that which minimizes the
26  sum of squared distances between corresponding points. This uses the
27  Kabsch algorithm.
28 
29  \return the
30  \f[ \textit{argmin}_T
31  \sum \left|T\left(f\left[i\right]\right)-t[i]\right|^2 \f]
32 
33  If the point sets lie in a 1 or 2 dimensional subspace,
34  the alignment algorithm
35  is unstable and not guaranteed to work. A warning is printed in this
36  case.
37 
38  \genericgeometry
39 
40  \see Transformation3D
41  \see Vector3D
42  */
43 template <class Vector3DsOrXYZs0, class Vector3DsOrXYZs1>
45 get_transformation_aligning_first_to_second(const Vector3DsOrXYZs0& source,
46  const Vector3DsOrXYZs1& target) {
47  using algebra::get_vector_geometry;
48  IMP_INTERNAL_CHECK(source.size() == target.size(), "sizes don't match");
49  IMP_INTERNAL_CHECK(source.size() > 0, "Points are needed");
50  // compute the centroid of the points and transform
51  // point sets so that their centroids coincide
52 
53  Vector3D center_source(0, 0, 0), center_target(0, 0, 0);
54  for (unsigned int i = 0; i < source.size(); ++i) {
55  // double x= p_it->x();
56  center_source += get_vector_geometry(source[i]);
57  center_target += get_vector_geometry(target[i]);
58  IMP_LOG_VERBOSE(i << ": (" << get_vector_geometry(source[i]) << ") ("
59  << get_vector_geometry(target[i]) << ")\n");
60  }
61  center_source = center_source / source.size();
62  center_target = center_target / target.size();
63 
64  IMP_LOG_VERBOSE("Centers are (" << center_source << ") (" << center_target
65  << ")\n");
66  Vector3Ds shifted_source(source.size()), shifted_target(target.size());
67  for (unsigned int i = 0; i < source.size(); ++i) {
68  shifted_source[i] = get_vector_geometry(source[i]) - center_source;
69  shifted_target[i] = get_vector_geometry(target[i]) - center_target;
70  }
71 
72  // covariance matrix
73  Eigen::Matrix3d H;
74  for (int i = 0; i < 3; i++) {
75  for (int j = 0; j < 3; j++) {
76  H(i, j) = 0;
77  }
78  }
79  for (unsigned int i = 0; i < source.size(); i++) {
80  for (int j = 0; j < 3; j++) {
81  for (int k = 0; k < 3; k++) {
82  H(j, k) += shifted_source[i][j] * shifted_target[i][k];
83  }
84  }
85  }
86 
87  IMP_LOG_VERBOSE("H is " << H << std::endl);
88  Eigen::JacobiSVD<Eigen::Matrix3d> svd =
89  H.jacobiSvd(Eigen::ComputeFullV | Eigen::ComputeFullU);
90  Eigen::Matrix3d U = svd.matrixU(), V = svd.matrixV();
91  Eigen::Vector3d SV = svd.singularValues();
92  IMP_LOG_VERBOSE("SVD is " << U << std::endl << V << std::endl);
93 
94  double det = SV[0] * SV[1] * SV[2];
96  if (det < .00001) {
97  IMP_LOG_TERSE("SOURCE:\n");
98  for (unsigned int i = 0; i < source.size(); ++i) {
99  IMP_LOG_TERSE(source[i] << std::endl);
100  }
101  IMP_LOG_TERSE("TO:\n");
102  for (unsigned int i = 0; i < target.size(); ++i) {
103  IMP_LOG_TERSE(target[i] << std::endl);
104  }
105  IMP_LOG_TERSE(H);
106  IMP_WARN("Degenerate point set. I may not be able to align them."
107  << std::endl);
108  }
109  }
110 
112  Eigen::Matrix3d Sigma = Eigen::Matrix3d::Zero();
113 
114  for (int i = 0; i < 3; ++i) {
115  Sigma(i, i) = SV[i];
116  }
117 
118  IMP_LOG_VERBOSE("Reconstructed is " << U * Sigma * V.transpose()
119  << std::endl);
120  }
121 
122  // the rotation matrix is R = VU^T
123  Eigen::Matrix3d rot = V * U.transpose();
124 
125  // check for reflection
126  if (rot.determinant() < 0) {
127  IMP_LOG_VERBOSE("Flipping matrix" << std::endl);
128  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
129  S(0, 0) = S(1, 1) = 1;
130  S(2, 2) = -1;
131  rot = V * S * U.transpose();
132  }
133 
134  IMP_LOG_VERBOSE("Rotation matrix is " << rot << std::endl);
135 
136  Rotation3D rotation = get_rotation_from_matrix(rot);
137  IMP_LOG_VERBOSE("Rotation is " << rotation << std::endl);
138  Vector3D translation = center_target - rotation.get_rotated(center_source);
139 
140  Transformation3D ret(rotation, translation);
141  return ret;
142 }
143 
144 //! Builds the transformation required to obtain a set of points from
145 //! the first one
146 /**
147  \note The function assumes that the relative distances between points
148  are conserved.
149 **/
150 IMPALGEBRAEXPORT Transformation2D
152  const Vector2Ds& set_to);
153 
154 // implemented in Transformation2D
155 
156 IMPALGEBRA_END_NAMESPACE
157 
158 #endif /* IMPALGEBRA_GEOMETRIC_ALIGNMENT_H */
Vector3D get_rotated(const Vector3D &o) const
Rotate a vector around the origin.
Definition: Rotation3D.h:178
Transformation2D get_transformation_aligning_pair(const Vector2Ds &set_from, const Vector2Ds &set_to)
Simple 3D transformation class.
#define IMP_IF_CHECK(level)
Execute the code block if a certain level checks are on.
Definition: check_macros.h:104
2D transformations. Copyright 2007-2019 IMP Inventors. All rights reserved.
Simple 3D sphere class.
#define IMP_LOG_VERBOSE(expr)
Definition: log_macros.h:94
A more IMP-like version of the std::vector.
Definition: Vector.h:39
#define IMP_LOG_TERSE(expr)
Definition: log_macros.h:83
#define IMP_INTERNAL_CHECK(expr, message)
An assertion to check for internal errors in IMP. An IMP::ErrorException will be thrown.
Definition: check_macros.h:139
Vector< VectorD< 2 > > Vector2Ds
Definition: VectorD.h:419
IMP::algebra::Transformation3D get_transformation_aligning_first_to_second(const Vector3DsOrXYZs0 &source, const Vector3DsOrXYZs1 &target)
Compute the rigid transform bringing the first point set to the second.
#define IMP_WARN(expr)
Write a warning to a log.
Definition: log_macros.h:66
Logging and error reporting support.
#define IMP_IF_LOG(level)
Execute the code block if a certain level of logging is on.
Definition: log_macros.h:60
Simple 3D rotation class.
Simple 3D transformation class.
3D rotation class.
Definition: Rotation3D.h:51
Rotation3D get_rotation_from_matrix(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22)
Generate a Rotation3D object from a rotation matrix.
Exception definitions and assertions.
VectorD< 3 > Vector3D
Definition: VectorD.h:421
Simple 3D vector class.
Logging and error reporting support.