IMP  2.1.1
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-2013 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 "internal/tnt_array2d.h"
18 #include "internal/jama_svd.h"
19 #include <IMP/base/check_macros.h>
20 #include <IMP/base/log.h>
21 
22 IMPALGEBRA_BEGIN_NAMESPACE
23 
24 //! Compute the rigid transform bringing the first point set to the second
25 /** The points are assumed to be corresponding (that is, from[0] is aligned
26  to to[0] etc.). The alignment computed is that which minimized the
27  sum of squared distances between corresponding points. Return the
28  \f[ \textit{argmin}_T
29  \sum \left|T\left(f\left[i\right]\right)-t[i]\right|^2 \f]
30 
31  If the point sets lie in a 1 or 2 dimensional subspace,
32  the alignment algorithm
33  is unstable and not guaranteed to work. A warning is printed in this
34  case.
35 
36  \genericgeometry
37 
38  See Transformation3D
39  \see Vector3D
40  */
41 template <class Vector3DsOrXYZs0, class Vector3DsOrXYZs1>
43 get_transformation_aligning_first_to_second(const Vector3DsOrXYZs0& source,
44  const Vector3DsOrXYZs1& target) {
45  IMP_INTERNAL_CHECK(source.size() == target.size(), "sizes don't match");
46  IMP_INTERNAL_CHECK(source.size() > 0, "Points are needed");
47  // compute the centroid of the points and transform
48  // pointsets so that their centroids coinside
49 
50  Vector3D center_source(0, 0, 0), center_target(0, 0, 0);
51  for (unsigned int i = 0; i < source.size(); ++i) {
52  // double x= p_it->x();
53  center_source += get_vector_d_geometry(source[i]);
54  center_target += get_vector_d_geometry(target[i]);
55  IMP_LOG_VERBOSE(i << ": (" << get_vector_d_geometry(source[i]) << ") ("
56  << get_vector_d_geometry(target[i]) << ")\n");
57  }
58  center_source = center_source / source.size();
59  center_target = center_target / target.size();
60 
61  IMP_LOG_VERBOSE("Centers are (" << center_source << ") (" << center_target
62  << ")\n");
63  Vector3Ds shifted_source(source.size()), shifted_target(target.size());
64  for (unsigned int i = 0; i < source.size(); ++i) {
65  shifted_source[i] = get_vector_d_geometry(source[i]) - center_source;
66  shifted_target[i] = get_vector_d_geometry(target[i]) - center_target;
67  }
68 
69  // covariance matrix
70  internal::TNT::Array2D<double> H(3, 3);
71  for (int i = 0; i < 3; i++) {
72  for (int j = 0; j < 3; j++) {
73  H[i][j] = 0;
74  }
75  }
76  for (unsigned int i = 0; i < source.size(); i++) {
77  for (int j = 0; j < 3; j++) {
78  for (int k = 0; k < 3; k++) {
79  H[j][k] += shifted_source[i][j] * shifted_target[i][k];
80  }
81  }
82  }
83 
84  IMP_LOG_VERBOSE("H is " << H << std::endl);
85 
86  internal::JAMA::SVD<double> svd(H);
87  internal::TNT::Array2D<double> U(3, 3), V(3, 3);
88  svd.getU(U);
89  svd.getV(V);
90 
91  IMP_LOG_VERBOSE("SVD is " << U << std::endl << V << std::endl);
92 
93  internal::TNT::Array1D<double> SV;
94  svd.getSingularValues(SV);
95  double det = SV[0] * SV[1] * SV[2];
97  if (det < .00001) {
98  IMP_LOG_TERSE("SOURCE:\n");
99  for (unsigned int i = 0; i < source.size(); ++i) {
100  IMP_LOG_TERSE(source[i] << std::endl);
101  }
102  IMP_LOG_TERSE("TO:\n");
103  for (unsigned int i = 0; i < source.size(); ++i) {
104  IMP_LOG_TERSE(target[i] << std::endl);
105  }
106  IMP_LOG_TERSE(H);
107  IMP_WARN("Degenerate point set. I may not be able to align them."
108  << std::endl);
109  }
110  }
111 
113  internal::TNT::Array2D<double> Sigma(3, 3, 0.0);
114 
115  for (int i = 0; i < 3; ++i) {
116  Sigma[i][i] = SV[i];
117  }
118 
119  IMP_LOG_VERBOSE("Reconstructed is " << internal::TNT::matmult(
120  internal::TNT::matmult(U, Sigma),
121  internal::TNT::transpose(V))
122  << std::endl);
123  }
124 
125  // the rotation matrix is R = VU^T
126  internal::TNT::Array2D<double> UT = internal::TNT::transpose(U);
127  internal::TNT::Array2D<double> rot(3, 3);
128  rot = matmult(V, UT);
129 
130  // check for reflection
131  if (determinant(rot) < 0) {
132  IMP_LOG_VERBOSE("Flipping matrix" << std::endl);
133  internal::TNT::Array2D<double> VT = internal::TNT::transpose(V);
134  internal::TNT::Array2D<double> UVT = internal::TNT::matmult(U, VT);
135  internal::TNT::Array2D<double> S(3, 3);
136  S[0][0] = S[1][1] = 1;
137  S[2][2] = determinant(UVT);
138  S[0][1] = S[0][2] = S[1][0] = S[1][2] = S[2][0] = S[2][1] = 0;
139  rot = internal::TNT::matmult(internal::TNT::matmult(U, S), VT);
140  }
141 
142  IMP_LOG_VERBOSE("Rotation matrix is " << rot << std::endl);
143 
145  rot[0][0], rot[0][1], rot[0][2], rot[1][0], rot[1][1], rot[1][2],
146  rot[2][0], rot[2][1], rot[2][2]);
147  IMP_LOG_VERBOSE("Rotation is " << rotation << std::endl);
148  Vector3D translation = center_target - rotation.get_rotated(center_source);
149 
150  Transformation3D ret(rotation, translation);
151  return ret;
152 }
153 
154 //! Builds the transformation required to obtain a set of points from
155 //! the first one
156 /**
157  \note The function assumes that the relative distances between points
158  are conserved.
159 **/
160 IMPALGEBRAEXPORT Transformation2D
162  const Vector2Ds& set_to);
163 
164 // implemented in Transformation2D
165 
166 IMPALGEBRA_END_NAMESPACE
167 
168 #endif /* IMPALGEBRA_GEOMETRIC_ALIGNMENT_H */
Vector3D get_rotated(const Vector3D &o) const
Rotate a vector around the origin.
Definition: Rotation3D.h:148
Transformation2D get_transformation_aligning_pair(const Vector2Ds &set_from, const Vector2Ds &set_to)
Simple 3D transformation class.
#define IMP_WARN(expr)
Write a warning to a log.
base::Vector< VectorD< 2 > > Vector2Ds
Definition: VectorD.h:585
2D transformations. Copyright 2007-2013 IMP Inventors. All rights reserved.
Simple 3D sphere class.
#define IMP_LOG_TERSE(expr)
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_INTERNAL_CHECK(expr, message)
An assertion to check for internal errors in IMP. An IMP::ErrorException will be thrown.
#define IMP_IF_CHECK(level)
Execute the code block if a certain level checks are on.
Simple 3D rotation class.
Simple 3D transformation class.
3D rotation class.
Definition: Rotation3D.h:45
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.
Logging and error reporting support.
Simple 3D vector class.
Exception definitions and assertions.
#define IMP_IF_LOG(level)
Execute the code block if a certain level of logging is on.
#define IMP_LOG_VERBOSE(expr)