IMP  2.2.0
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-2014 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/base/check_macros.h>
18 #include <IMP/base/log.h>
19 #include <IMP/base/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 minimized the
26  sum of squared distances between corresponding points. Return the
27  \f[ \textit{argmin}_T
28  \sum \left|T\left(f\left[i\right]\right)-t[i]\right|^2 \f]
29 
30  If the point sets lie in a 1 or 2 dimensional subspace,
31  the alignment algorithm
32  is unstable and not guaranteed to work. A warning is printed in this
33  case.
34 
35  \genericgeometry
36 
37  See Transformation3D
38  \see Vector3D
39  */
40 template <class Vector3DsOrXYZs0, class Vector3DsOrXYZs1>
42 get_transformation_aligning_first_to_second(const Vector3DsOrXYZs0& source,
43  const Vector3DsOrXYZs1& target) {
44  using algebra::get_vector_geometry;
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_geometry(source[i]);
54  center_target += get_vector_geometry(target[i]);
55  IMP_LOG_VERBOSE(i << ": (" << get_vector_geometry(source[i]) << ") ("
56  << get_vector_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_geometry(source[i]) - center_source;
66  shifted_target[i] = get_vector_geometry(target[i]) - center_target;
67  }
68 
69  // covariance matrix
70  IMP_Eigen::Matrix3d H;
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  IMP_Eigen::JacobiSVD<IMP_Eigen::Matrix3d> svd =
86  H.jacobiSvd(IMP_Eigen::ComputeFullV | IMP_Eigen::ComputeFullU);
87  IMP_Eigen::Matrix3d U = svd.matrixU(), V = svd.matrixV();
88  IMP_Eigen::Vector3d SV = svd.singularValues();
89  IMP_LOG_VERBOSE("SVD is " << U << std::endl << V << std::endl);
90 
91  double det = SV[0] * SV[1] * SV[2];
93  if (det < .00001) {
94  IMP_LOG_TERSE("SOURCE:\n");
95  for (unsigned int i = 0; i < source.size(); ++i) {
96  IMP_LOG_TERSE(source[i] << std::endl);
97  }
98  IMP_LOG_TERSE("TO:\n");
99  for (unsigned int i = 0; i < source.size(); ++i) {
100  IMP_LOG_TERSE(target[i] << std::endl);
101  }
102  IMP_LOG_TERSE(H);
103  IMP_WARN("Degenerate point set. I may not be able to align them."
104  << std::endl);
105  }
106  }
107 
109  IMP_Eigen::Matrix3d Sigma = IMP_Eigen::Matrix3d::Zero();
110 
111  for (int i = 0; i < 3; ++i) {
112  Sigma(i, i) = SV[i];
113  }
114 
115  IMP_LOG_VERBOSE("Reconstructed is " << U * Sigma * V.transpose()
116  << std::endl);
117  }
118 
119  // the rotation matrix is R = VU^T
120  IMP_Eigen::Matrix3d rot = V * U.transpose();
121 
122  // check for reflection
123  if (rot.determinant() < 0) {
124  IMP_LOG_VERBOSE("Flipping matrix" << std::endl);
125  IMP_Eigen::Matrix3d S = IMP_Eigen::Matrix3d::Zero();
126  S(0, 0) = S(1, 1) = 1;
127  S(2, 2) = (U * V.transpose()).determinant();
128  rot = U * S * V.transpose();
129  }
130 
131  IMP_LOG_VERBOSE("Rotation matrix is " << rot << std::endl);
132 
133  Rotation3D rotation = get_rotation_from_matrix(rot);
134  IMP_LOG_VERBOSE("Rotation is " << rotation << std::endl);
135  Vector3D translation = center_target - rotation.get_rotated(center_source);
136 
137  Transformation3D ret(rotation, translation);
138  return ret;
139 }
140 
141 //! Builds the transformation required to obtain a set of points from
142 //! the first one
143 /**
144  \note The function assumes that the relative distances between points
145  are conserved.
146 **/
147 IMPALGEBRAEXPORT Transformation2D
149  const Vector2Ds& set_to);
150 
151 // implemented in Transformation2D
152 
153 IMPALGEBRA_END_NAMESPACE
154 
155 #endif /* IMPALGEBRA_GEOMETRIC_ALIGNMENT_H */
Vector3D get_rotated(const Vector3D &o) const
Rotate a vector around the origin.
Definition: Rotation3D.h:154
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:393
2D transformations. Copyright 2007-2014 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.
Logging and error reporting support.
#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:46
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.
VectorD< 3 > Vector3D
Definition: VectorD.h:395
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)