absOrientation.cpp
Go to the documentation of this file.
1 //-----------------------------------------------------------------------------------
2 // d-SEAMS is free software: you can redistribute it and/or modify
3 // it under the terms of the GNU General Public License as published by
4 // the Free Software Foundation, either version 3 of the License, or
5 // (at your option) any later version.
6 //
7 // A copy of the GNU General Public License is available at
8 // http://www.gnu.org/licenses/
9 //-----------------------------------------------------------------------------------
10 
11 #include <absOrientation.hpp>
12 
26 int absor::hornAbsOrientation(const Eigen::MatrixXd &refPoints,
27  const Eigen::MatrixXd &targetPoints,
28  std::vector<double> *quat, double *rmsd,
29  std::vector<double> *rmsdList, double *scale) {
30  int nop =
31  refPoints.rows(); // Number of particles (equal to the number of rows)
32  int dim =
33  refPoints.cols(); // Number of dimensions (equal to the number of columns)
34  Eigen::MatrixXd centeredRefPnts(
35  nop, dim); // Reference point set after centering wrt the centroid
36  Eigen::MatrixXd centeredTargetPnts(
37  nop, dim); // Target point set after centering wrt the centroid
38  Eigen::MatrixXd S(dim,
39  dim); // Matrix containing sums of products of coordinates
40  Eigen::MatrixXd N(
41  4, 4); // 4x4 Matrix, whose largest eigenvector must be calculated
42  Eigen::VectorXd calcEigenVec(
43  4); // This should have 4 components (eigen vector calculated from N)
44  // -----
45  // Check that the sizes of the reference point set (right point system) and
46  // the target point set (left point system) are the same
47  if (refPoints.rows() != targetPoints.rows() ||
48  refPoints.cols() != targetPoints.cols()) {
49  // Throw error
50  std::cerr
51  << "The reference and target point sets are not of the same size.\n";
52  return 1;
53  } // unequal size; error!
54  // -----
55  //
56  // ---------------------------------------------------
57  // FINDING THE CENTROIDS AND THE NEW COORDINATES WRT THE CENTROIDS
58  centeredRefPnts = absor::centerWRTcentroid(refPoints);
59  centeredTargetPnts = absor::centerWRTcentroid(targetPoints);
60  // ---------------------------------------------------
61  // FINDING THE ROTATION MATRIX
62  //
63  // Find the 3x3 matrix S
64  S = absor::calcMatrixS(centeredRefPnts, centeredTargetPnts, nop, dim);
65  // Calculate the 4x4 symmetric matrix N, whose largest eigenvector yields the
66  // quaternion in the same direction
67  N = absor::calcMatrixN(S);
68  // --------
69  // Calculate the eigenvector corresponding to the largest eigenvalue
70  //
71  // Construct matrix operation object (op) using the wrapper class
72  // DenseSymMatProd for the matrix N
73  Spectra::DenseSymMatProd<double> op(N);
74  //
75  // Construct eigen solver object, requesting the largest 1 eigenvalue and
76  // eigenvector
77  Spectra::SymEigsSolver<double, Spectra::LARGEST_ALGE,
78  Spectra::DenseSymMatProd<double>>
79  eigs(&op, 1, 4);
80  //
81  // Initialize and compute
82  eigs.init();
83  int nconv = eigs.compute();
84  // Get the eigenvalue and eigenvector
85  if (eigs.info() == Spectra::SUCCESSFUL) {
86  Eigen::VectorXd calcEigenValue = eigs.eigenvalues(); // Eigenvalue
87  calcEigenVec = eigs.eigenvectors();
88  } // end of eigenvector calculation
89  //
90  // --------
91  // Normalize the eigenvector calculated
92  double qNorm = sqrt(calcEigenVec.dot(calcEigenVec));
93  calcEigenVec /= qNorm; // Divide by the square root of the sum
94  // Update the quaternion with the normalized eigenvector
95  (*quat).resize(4); // Output quaternion update
96  for (int i = 0; i < 4; i++) {
97  (*quat)[i] = calcEigenVec(i);
98  } // end of quaternion update
99  // --------
100  // ---------------------------------------------------
101  // COMPUTE THE OPTIMUM SCALE
102  (*scale) = absor::calcScaleFactor(centeredRefPnts, centeredTargetPnts, nop);
103  // ---------------------------------------------------
104  // GETTING THE ERROR
105  (*rmsd) = absor::getRMSD(centeredRefPnts, centeredTargetPnts, calcEigenVec,
106  rmsdList, nop, (*scale));
107  // ---------------------------------------------------
108  return 0;
109 } // end of function
110 
127 Eigen::MatrixXd absor::calcMatrixS(const Eigen::MatrixXd &centeredRefPnts,
128  const Eigen::MatrixXd &centeredTargetPnts,
129  int nop, int dim) {
130  Eigen::MatrixXd S(nop, dim); // Output matrix S
131  Eigen::VectorXd targetCoord(nop); // Column of the target point set
132  Eigen::VectorXd refCoord(nop); // Column of the reference point set
133  double Svalue; // Current value being filled (Sxx, Sxy etc.)
134 
135  // Calculate Sxx, Sxy, Sxz etc
136  for (int iCol = 0; iCol < dim; iCol++) {
137  //
138  for (int jCol = 0; jCol < dim; jCol++) {
139  targetCoord =
140  centeredTargetPnts.col(iCol); // iCol^th column of target point set
141  refCoord = centeredRefPnts.col(jCol); // jCol^th of reference point set
142  Svalue = targetCoord.dot(refCoord);
143  S(iCol, jCol) = Svalue;
144  } // end column wise filling
145  } // end of filling
146 
147  // Output matrix
148  return S;
149 } // end of function
150 
168 Eigen::MatrixXd absor::calcMatrixN(const Eigen::MatrixXd &S) {
169  //
170  Eigen::MatrixXd N(4, 4); // Output matrix N
171  // Components of S
172  double Sxx = S(0, 0);
173  double Sxy = S(0, 1);
174  double Sxz = S(0, 2);
175  double Syx = S(1, 0);
176  double Syy = S(1, 1);
177  double Syz = S(1, 2);
178  double Szx = S(2, 0);
179  double Szy = S(2, 1);
180  double Szz = S(2, 2);
181 
182  // N=[(Sxx+Syy+Szz) (Syz-Szy) (Szx-Sxz) (Sxy-Syx);...
183  // (Syz-Szy) (Sxx-Syy-Szz) (Sxy+Syx) (Szx+Sxz);...
184  // (Szx-Sxz) (Sxy+Syx) (-Sxx+Syy-Szz) (Syz+Szy);...
185  // (Sxy-Syx) (Szx+Sxz) (Syz+Szy) (-Sxx-Syy+Szz)];
186 
187  // ------------------
188  // Calculating N:
189  // Diagonal elements
190  N(0, 0) = Sxx + Syy + Szz;
191  N(1, 1) = Sxx - Syy - Szz;
192  N(2, 2) = -Sxx + Syy - Szz;
193  N(3, 3) = -Sxx - Syy + Szz;
194  // Other elements
195  // First row
196  N(0, 1) = Syz - Szy;
197  N(0, 2) = Szx - Sxz;
198  N(0, 3) = Sxy - Syx;
199  // Second row
200  N(1, 0) = Syz - Szy;
201  N(1, 2) = Sxy + Syx;
202  N(1, 3) = Szx + Sxz;
203  // Third row
204  N(2, 0) = Szx - Sxz;
205  N(2, 1) = Sxy + Syx;
206  N(2, 3) = Syz + Szy;
207  // Fourth row
208  N(3, 0) = Sxy - Syx;
209  N(3, 1) = Szx + Sxz;
210  N(3, 2) = Syz + Szy;
211  // ------------------
212  // Output matrix
213  return N;
214 } // end of function
215 
224 Eigen::MatrixXd absor::centerWRTcentroid(const Eigen::MatrixXd &pointSet) {
225  int nop = pointSet.rows(); // Number of particles
226  int dim = pointSet.cols(); // Number of dimensions
227  Eigen::MatrixXd centeredPointSet(nop, dim); // Output point set
228  Eigen::VectorXd vecOfOnes(nop); // vector of ones
229  std::vector<double> centroid;
230  double coordValue;
231  double centeredVal;
232  //
233  centroid.resize(dim); // Init to zero
234  vecOfOnes = Eigen::VectorXd::Ones(nop);
235  // --------------------------------
236 
237  for (int i = 0; i < nop; i++) {
238  for (int k = 0; k < dim; k++) {
239  coordValue = pointSet(i, k);
240  centroid[k] += coordValue;
241  } // loop through columns
242  } // end of loop through rows
243  // Divide by the total number of particles
244  centroid[0] /= nop; // x
245  centroid[1] /= nop; // y
246  centroid[2] /= nop; // z
247  // --------------------------------
248  // Subtract the centroid from the coordinates to get the centered point set
249  for (int i = 0; i < nop; i++) {
250  for (int k = 0; k < dim; k++) {
251  coordValue = pointSet(i, k);
252  centeredVal = coordValue - centroid[k];
253  centeredPointSet(i, k) = centeredVal;
254  } // end of loop through columns (dimensions)
255  } // end of loop through the rows
256  // --------------------------------
257  return centeredPointSet;
258 } // end of function
259 
272 double absor::calcScaleFactor(const Eigen::MatrixXd &rightSys,
273  const Eigen::MatrixXd &leftSys, int n) {
274  double scale; // Output scale
275  double v1, v2; // Sum of the length of the vector
276  Eigen::VectorXd rightVec(
277  3); // Vector of the i^th particle in the right system
278  Eigen::VectorXd leftVec(3); // Vector of the i^th particle in the right system
279 
280  // scale = (sigma_to_n ||r_r||^2 / ||r_l||^2)^0.5
281  // ref: http://people.csail.mit.edu/bkph/papers/Absolute_Orientation.pdf
282 
283  // Loop through all the points, and get the dot product of the vector for each
284  // point
285  for (int i = 0; i < n; i++) {
286  //
287  rightVec = rightSys.row(i); // i^th row of the right system
288  leftVec = leftSys.row(i); // i^th row of the left system
289  v1 += rightVec.dot(rightVec);
290  v2 += leftVec.dot(leftVec);
291  } // end of loop through all points
292 
293  // The optimum scale is the ratio of v1 and v2
294  scale = std::sqrt(v1 / v2);
295 
296  return scale;
297 } // end of function
298 
305 Eigen::MatrixXd absor::quat2RotMatrix(const Eigen::VectorXd &quat) {
306  //
307  Eigen::MatrixXd R(3, 3); // Rotation matrix
308  // Components of the quaternion
309  double q0 = quat(0);
310  double qx = quat(1);
311  double qy = quat(2);
312  double qz = quat(3);
313 
314  // Quaternion derived rotation matrix, when q (q=q0+qx*i+qy*j+qz*k)
315  // is a unit quaternion:
316  // R=[(q0^2+qx^2+qy^2+qz^2) 2(qx*qy-q0*qz) 2(qx*qz+q0*qy);...
317  // 2(qy*qx-q0*qz) (q0^2+qx^2+qy^2+qz^2) 2(qy*qz-q0*qx);...
318  // 2(qz*qx-q0*qy) 2(qz*qy+q0*qz) (q0^2+qx^2+qy^2+qz^2)];
319 
320  // Fill up the rotation matrix R according to the above formula
321  //
322  // First row
323  R(0, 0) = q0 * q0 + qx * qx + qy * qy + qz * qz;
324  R(0, 1) = 2 * (qx * qy - q0 * qz);
325  R(0, 2) = 2 * (qx * qz + q0 * qy);
326  // Second row
327  R(1, 0) = 2 * (qy * qx + q0 * qz);
328  R(1, 1) = q0 * q0 + qx * qx + qy * qy + qz * qz;
329  R(1, 2) = 2 * (qy * qz - q0 * qy);
330  // Third row
331  R(2, 0) = 2 * (qz * qx - q0 * qy);
332  R(2, 1) = 2 * (qz * qy + q0 * qz);
333  R(2, 2) = q0 * q0 + qx * qx + qy * qy + qz * qz;
334 
335  // return the rotation matrix
336  return R;
337 } // end of function
338 
355 double absor::getRMSD(const Eigen::MatrixXd &centeredRefPnts,
356  const Eigen::MatrixXd &centeredTargetPnts,
357  const Eigen::VectorXd &quat,
358  std::vector<double> *rmsdList, int nop, double scale) {
359  Eigen::MatrixXd R(3, 3); // The (3x3) rotation vector
360  // The RMSD per atom is filled in this vector
361  (*rmsdList).resize(nop);
362  //
364  quat); // orthonormal rotation matrix from the quaternion
365  // The total error is:
366  // sum_over_all_n (r'_r - s*rotated_r_l')
367  double rmsd = 0.0; // Error
368  Eigen::VectorXd errorVec(3); // The vector which is the r_r -s*R
369  Eigen::VectorXd rotatedLeft(3);
370  Eigen::VectorXd targetCol(3);
371  Eigen::VectorXd refCol(3);
372  //
373  for (int i = 0; i < nop; i++) {
374  //
375  // Rotate the left system coordinate using the rotation matrix
376  targetCol = (centeredTargetPnts.row(i)).transpose();
377  refCol = (centeredRefPnts.row(i)).transpose();
378  //
379  rotatedLeft = R * targetCol;
380  errorVec = refCol - scale * rotatedLeft;
381  rmsd += errorVec.dot(errorVec); // Total error
382  (*rmsdList)[i] = sqrt(errorVec.dot(errorVec)); // Error per atom
383  } // end of loop through every row
384  //
385  return sqrt(rmsd / nop);
386 } // end of function
double getRMSD(const Eigen::MatrixXd &centeredRefPnts, const Eigen::MatrixXd &centeredTargetPnts, const Eigen::VectorXd &quat, std::vector< double > *rmsdList, int nop, double scale)
Calculate the RMSD.
Eigen::MatrixXd calcMatrixS(const Eigen::MatrixXd &centeredRefPnts, const Eigen::MatrixXd &centeredTargetPnts, int nop, int dim)
double calcScaleFactor(const Eigen::MatrixXd &rightSys, const Eigen::MatrixXd &leftSys, int n)
Calculate the scale factor from the centered left and right point sets.
Eigen::MatrixXd centerWRTcentroid(const Eigen::MatrixXd &pointSet)
Center a point set wrt the centroid.
int hornAbsOrientation(const Eigen::MatrixXd &refPoints, const Eigen::MatrixXd &targetPoints, std::vector< double > *quat, double *rmsd, std::vector< double > *rmsdList, double *scale)
Get the absolute orientation using Horn's algorithm (with quaternions)
Eigen::MatrixXd calcMatrixN(const Eigen::MatrixXd &S)
Eigen::MatrixXd quat2RotMatrix(const Eigen::VectorXd &quat)
Get a rotation matrix from a unit quaternion.
T resize(T... args)
T sqrt(T... args)