Instrument Neutral Distributed Interface INDI  2.0.2
SVDMathPlugin.cpp
Go to the documentation of this file.
1 
5 #include "SVDMathPlugin.h"
6 
7 #include "DriverCommon.h"
8 
9 #include <gsl/gsl_linalg.h>
10 
11 namespace INDI
12 {
13 namespace AlignmentSubsystem
14 {
15 // Standard functions required for all plugins
16 extern "C" {
17  SVDMathPlugin *Create()
18  {
19  return new SVDMathPlugin;
20  }
21 
22  void Destroy(SVDMathPlugin *pPlugin)
23  {
24  delete pPlugin;
25  }
26 
27  const char *GetDisplayName()
28  {
29  return "SVD Math Plugin";
30  }
31 }
32 void SVDMathPlugin::CalculateTransformMatrices(const TelescopeDirectionVector &Alpha1,
33  const TelescopeDirectionVector &Alpha2,
34  const TelescopeDirectionVector &Alpha3,
35  const TelescopeDirectionVector &Beta1,
36  const TelescopeDirectionVector &Beta2,
37  const TelescopeDirectionVector &Beta3, gsl_matrix *pAlphaToBeta,
38  gsl_matrix *pBetaToAlpha)
39 {
40  // Set up the column vectors
41  gsl_matrix *pAlphaMatrix = gsl_matrix_alloc(3, 3);
42  gsl_matrix_set(pAlphaMatrix, 0, 0, Alpha1.x);
43  gsl_matrix_set(pAlphaMatrix, 1, 0, Alpha1.y);
44  gsl_matrix_set(pAlphaMatrix, 2, 0, Alpha1.z);
45  gsl_matrix_set(pAlphaMatrix, 0, 1, Alpha2.x);
46  gsl_matrix_set(pAlphaMatrix, 1, 1, Alpha2.y);
47  gsl_matrix_set(pAlphaMatrix, 2, 1, Alpha2.z);
48  gsl_matrix_set(pAlphaMatrix, 0, 2, Alpha3.x);
49  gsl_matrix_set(pAlphaMatrix, 1, 2, Alpha3.y);
50  gsl_matrix_set(pAlphaMatrix, 2, 2, Alpha3.z);
51  Dump3x3("AlphaMatrix", pAlphaMatrix);
52  gsl_matrix *pBetaMatrix = gsl_matrix_alloc(3, 3);
53  gsl_matrix_set(pBetaMatrix, 0, 0, Beta1.x);
54  gsl_matrix_set(pBetaMatrix, 1, 0, Beta1.y);
55  gsl_matrix_set(pBetaMatrix, 2, 0, Beta1.z);
56  gsl_matrix_set(pBetaMatrix, 0, 1, Beta2.x);
57  gsl_matrix_set(pBetaMatrix, 1, 1, Beta2.y);
58  gsl_matrix_set(pBetaMatrix, 2, 1, Beta2.z);
59  gsl_matrix_set(pBetaMatrix, 0, 2, Beta3.x);
60  gsl_matrix_set(pBetaMatrix, 1, 2, Beta3.y);
61  gsl_matrix_set(pBetaMatrix, 2, 2, Beta3.z);
62  Dump3x3("BetaMatrix", pBetaMatrix);
63 
64  // Use Markley's singular value decomposition (SVD) method
65  // A detailed description can be found here
66  // http://www.control.auc.dk/~tb/best/aug23-Bak-svdalg.pdf
67 
68  // 1. Transpose the alpha matrix
69  gsl_matrix_transpose(pAlphaMatrix);
70 
71  // 2. Compute the first intermediate matrix
72  gsl_matrix *pIntermediateMatrix1 = gsl_matrix_alloc(3, 3);
73  MatrixMatrixMultiply(pBetaMatrix, pAlphaMatrix, pIntermediateMatrix1);
74 
75  // 3. Compute the singular value decomoposition of the intermediate matrix
76  gsl_matrix *pV = gsl_matrix_alloc(3, 3);
77  gsl_vector *pS = gsl_vector_alloc(3);
78  gsl_vector *pWork = gsl_vector_alloc(3);
79  gsl_linalg_SV_decomp(pIntermediateMatrix1, pV, pS, pWork);
80  // The intermediate matrix now contains the U matrix
81  // The V matrix is untransposed
82 
83  // 4. Compute the diagonal matrix
84  gsl_matrix *pDiagonal = gsl_matrix_calloc(3, 3);
85  gsl_matrix_set(pDiagonal, 0, 0, 1);
86  gsl_matrix_set(pDiagonal, 1, 1, 1);
87  gsl_matrix_set(pDiagonal, 2, 2, Matrix3x3Determinant(pIntermediateMatrix1) * Matrix3x3Determinant(pV));
88 
89  // 5. Compute the transform
90  gsl_matrix_transpose(pV);
91  gsl_matrix *pIntermediateMatrix2 = gsl_matrix_alloc(3, 3);
92  MatrixMatrixMultiply(pIntermediateMatrix1, pDiagonal, pIntermediateMatrix2);
93  MatrixMatrixMultiply(pIntermediateMatrix2, pV, pAlphaToBeta);
94 
95  Dump3x3("AlphaToBeta", pAlphaToBeta);
96 
97  if (nullptr != pBetaToAlpha)
98  {
99  // Invert the matrix to get the Apparent to Actual transform
100  if (!MatrixInvert3x3(pAlphaToBeta, pBetaToAlpha))
101  {
102  // pAlphaToBeta is singular and therefore is not a true transform
103  // and cannot be inverted. This probably means it contains at least
104  // one row or column that contains only zeroes
105  gsl_matrix_set_identity(pBetaToAlpha);
106  ASSDEBUG("CalculateTransformMatrices - AlphaToBeta matrix is singular!");
107  IDMessage(nullptr,
108  "Calculated Celestial to Telescope transformation matrix is singular (not a true transform).");
109  }
110 
111  Dump3x3("BetaToAlpha", pBetaToAlpha);
112  }
113 
114  // Clean up
115  gsl_matrix_free(pIntermediateMatrix1);
116  gsl_matrix_free(pV);
117  gsl_vector_free(pS);
118  gsl_vector_free(pWork);
119  gsl_matrix_free(pDiagonal);
120  gsl_matrix_free(pIntermediateMatrix2);
121  gsl_matrix_free(pBetaMatrix);
122  gsl_matrix_free(pAlphaMatrix);
123 }
124 
125 } // namespace AlignmentSubsystem
126 } // namespace INDI
#define ASSDEBUG(msg)
Definition: DriverCommon.h:17
bool MatrixInvert3x3(gsl_matrix *pInput, gsl_matrix *pInversion)
Calculate the inverse of the supplied matrix.
double Matrix3x3Determinant(gsl_matrix *pMatrix)
Caluclate the determinant of the supplied matrix.
void MatrixMatrixMultiply(gsl_matrix *pA, gsl_matrix *pB, gsl_matrix *pC)
Multiply matrix A by matrix B and put the result in C.
void Dump3x3(const char *Label, gsl_matrix *pMatrix)
Print out a 3x3 matrix to debug.
This class implements the SVD math plugin.
Definition: SVDMathPlugin.h:21
void IDMessage(const char *dev, const char *fmt,...)
Definition: indidriver.c:960
void Destroy(DummyMathPlugin *pPlugin)
DummyMathPlugin * Create()
Namespace to encapsulate INDI client, drivers, and mediator classes.