IMP  2.0.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 
23 IMPALGEBRA_BEGIN_NAMESPACE
24 
25 
26 //! Compute the rigid transform bringing the first point set to the second
27 /** The points are assumed to be corresponding (that is, from[0] is aligned
28  to to[0] etc.). The alignment computed is that which minimized the
29  sum of squared distances between corresponding points. 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  \relatesalso Transformation3D
41  \see Vector3D
42  */
43 template <class Vector3DsOrXYZs0,
44  class Vector3DsOrXYZs1>
47 const Vector3DsOrXYZs0 &source,
48 const Vector3DsOrXYZs1 &target
49 ) {
50  IMP_INTERNAL_CHECK(source.size() == target.size(), "sizes don't match");
51  IMP_INTERNAL_CHECK(source.size() >0, "Points are needed");
52  // compute the centroid of the points and transform
53  // pointsets so that their centroids coinside
54 
55  Vector3D center_source(0,0,0), center_target(0,0,0);
56  for (unsigned int i=0; i< source.size(); ++i) {
57  //double x= p_it->x();
58  center_source+= get_vector_d_geometry(source[i]);
59  center_target += get_vector_d_geometry(target[i]);
60  IMP_LOG_VERBOSE( i << ": (" << get_vector_d_geometry(source[i])
61  << ") (" << get_vector_d_geometry(target[i]) << ")\n");
62  }
63  center_source = center_source/source.size();
64  center_target = center_target/target.size();
65 
66  IMP_LOG_VERBOSE( "Centers are (" << center_source << ") (" << center_target
67  << ")\n");
68  Vector3Ds shifted_source(source.size()), shifted_target(target.size());
69  for (unsigned int i=0; i< source.size(); ++i) {
70  shifted_source[i]=get_vector_d_geometry(source[i])-center_source;
71  shifted_target[i]= get_vector_d_geometry(target[i])-center_target;
72  }
73 
74  // covariance matrix
75  internal::TNT::Array2D<double> H(3, 3);
76  for (int i = 0; i < 3; i++) {
77  for (int j = 0; j < 3; j++) {
78  H[i][j] = 0;
79  }
80  }
81  for (unsigned int i = 0; i < source.size(); i++) {
82  for (int j = 0; j < 3; j++) {
83  for (int k = 0; k < 3; k++) {
84  H[j][k] += shifted_source[i][j]*shifted_target[i][k];
85  }
86  }
87  }
88 
89  IMP_LOG_VERBOSE( "H is " << H << std::endl);
90 
91  internal::JAMA::SVD<double> svd(H);
92  internal::TNT::Array2D<double> U(3, 3), V(3, 3);
93  svd.getU(U);
94  svd.getV(V);
95 
96  IMP_LOG_VERBOSE( "SVD is " << U << std::endl << V << std::endl);
97 
98  internal::TNT::Array1D<double> SV;
99  svd.getSingularValues(SV);
100  double det= SV[0]*SV[1]*SV[2];
102  if (det < .00001) {
103  IMP_LOG_TERSE( "SOURCE:\n");
104  for (unsigned int i=0; i< source.size(); ++i) {
105  IMP_LOG_TERSE( source[i] << std::endl);
106  }
107  IMP_LOG_TERSE( "TO:\n");
108  for (unsigned int i=0; i< source.size(); ++i) {
109  IMP_LOG_TERSE( target[i] << std::endl);
110  }
111  IMP_LOG_TERSE( H);
112  IMP_WARN("Degenerate point set. I may not be able to align them."
113  << std::endl);
114  }
115  }
116 
118  internal::TNT::Array2D<double> Sigma(3,3, 0.0);
119 
120  for (int i=0; i < 3; ++i) {
121  Sigma[i][i]= SV[i];
122  }
123 
124  IMP_LOG_VERBOSE( "Reconstructed is "
125  << internal::TNT::matmult(internal::TNT::matmult(U,Sigma),
126  internal::TNT::transpose(V))
127  << std::endl);
128  }
129 
130  // the rotation matrix is R = VU^T
131  internal::TNT::Array2D<double> UT = internal::TNT::transpose(U);
132  internal::TNT::Array2D<double> rot(3, 3);
133  rot = matmult(V, UT);
134 
135  // check for reflection
136  if (determinant(rot) < 0) {
137  IMP_LOG_VERBOSE( "Flipping matrix"<<std::endl);
138  internal::TNT::Array2D<double> VT = internal::TNT::transpose(V);
139  internal::TNT::Array2D<double> UVT = internal::TNT::matmult(U, VT);
140  internal::TNT::Array2D<double> S(3, 3);
141  S[0][0] = S[1][1] = 1;
142  S[2][2] = determinant(UVT);
143  S[0][1] = S[0][2] = S[1][0] = S[1][2] = S[2][0] = S[2][1] = 0;
144  rot = internal::TNT::matmult(internal::TNT::matmult(U, S), VT);
145  }
146 
147  IMP_LOG_VERBOSE( "Rotation matrix is " << rot << std::endl);
148 
149  Rotation3D rotation
150  = get_rotation_from_matrix(rot[0][0], rot[0][1], rot[0][2],
151  rot[1][0], rot[1][1], rot[1][2],
152  rot[2][0], rot[2][1], rot[2][2]);
153  IMP_LOG_VERBOSE( "Rotation is " << rotation << std::endl);
154  Vector3D translation=center_target - rotation.get_rotated(center_source);
155 
156  Transformation3D ret(rotation, translation);
157  return ret;
158 }
159 
160 
161 
162 //! Builds the transformation required to obtain a set of points from
163 //! the first one
164 /**
165  \note The function assumes that the relative distances between points
166  are conserved.
167 **/
168 IMPALGEBRAEXPORT Transformation2D get_transformation_aligning_pair(
169  const Vector2Ds& set_from,
170  const Vector2Ds& set_to);
171 
172 // implemented in Transformation2D
173 
174 IMPALGEBRA_END_NAMESPACE
175 
176 #endif /* IMPALGEBRA_GEOMETRIC_ALIGNMENT_H */