8 #ifndef IMPALGEBRA_GEOMETRIC_ALIGNMENT_H
9 #define IMPALGEBRA_GEOMETRIC_ALIGNMENT_H
11 #include <IMP/algebra/algebra_config.h>
21 IMPALGEBRA_BEGIN_NAMESPACE
43 template <
class Vector3DsOrXYZs0,
class Vector3DsOrXYZs1>
46 const Vector3DsOrXYZs1& target) {
47 using algebra::get_vector_geometry;
53 Vector3D center_source(0, 0, 0), center_target(0, 0, 0);
54 for (
unsigned int i = 0; i < source.size(); ++i) {
56 center_source += get_vector_geometry(source[i]);
57 center_target += get_vector_geometry(target[i]);
59 << get_vector_geometry(target[i]) <<
")\n");
61 center_source = center_source / source.size();
62 center_target = center_target / target.size();
64 IMP_LOG_VERBOSE(
"Centers are (" << center_source <<
") (" << center_target
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;
74 for (
int i = 0; i < 3; i++) {
75 for (
int j = 0; j < 3; j++) {
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];
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();
94 double det = SV[0] * SV[1] * SV[2];
98 for (
unsigned int i = 0; i < source.size(); ++i) {
102 for (
unsigned int i = 0; i < target.size(); ++i) {
106 IMP_WARN(
"Degenerate point set. I may not be able to align them."
112 Eigen::Matrix3d Sigma = Eigen::Matrix3d::Zero();
114 for (
int i = 0; i < 3; ++i) {
123 Eigen::Matrix3d rot = V * U.transpose();
126 if (rot.determinant() < 0) {
128 Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
129 S(0, 0) = S(1, 1) = 1;
131 rot = V * S * U.transpose();
150 IMPALGEBRAEXPORT Transformation2D
156 IMPALGEBRA_END_NAMESPACE
Vector3D get_rotated(const Vector3D &o) const
Rotate a vector around the origin.
Transformation2D get_transformation_aligning_pair(const Vector2Ds &set_from, const Vector2Ds &set_to)
#define IMP_IF_CHECK(level)
Execute the code block if a certain level checks are on.
#define IMP_LOG_VERBOSE(expr)
A more IMP-like version of the std::vector.
#define IMP_LOG_TERSE(expr)
#define IMP_INTERNAL_CHECK(expr, message)
An assertion to check for internal errors in IMP. An IMP::ErrorException will be thrown.
Vector< VectorD< 2 > > Vector2Ds
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.
Logging and error reporting support.
#define IMP_IF_LOG(level)
Execute the code block if a certain level of logging is on.
Simple 3D rotation class.
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.
Logging and error reporting support.