Instrument Neutral Distributed Interface INDI  1.9.5
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  int GslRetcode;
41 
42  // Set up the column vectors
43  gsl_matrix *pAlphaMatrix = gsl_matrix_alloc(3, 3);
44  gsl_matrix_set(pAlphaMatrix, 0, 0, Alpha1.x);
45  gsl_matrix_set(pAlphaMatrix, 1, 0, Alpha1.y);
46  gsl_matrix_set(pAlphaMatrix, 2, 0, Alpha1.z);
47  gsl_matrix_set(pAlphaMatrix, 0, 1, Alpha2.x);
48  gsl_matrix_set(pAlphaMatrix, 1, 1, Alpha2.y);
49  gsl_matrix_set(pAlphaMatrix, 2, 1, Alpha2.z);
50  gsl_matrix_set(pAlphaMatrix, 0, 2, Alpha3.x);
51  gsl_matrix_set(pAlphaMatrix, 1, 2, Alpha3.y);
52  gsl_matrix_set(pAlphaMatrix, 2, 2, Alpha3.z);
53  Dump3x3("AlphaMatrix", pAlphaMatrix);
54  gsl_matrix *pBetaMatrix = gsl_matrix_alloc(3, 3);
55  gsl_matrix_set(pBetaMatrix, 0, 0, Beta1.x);
56  gsl_matrix_set(pBetaMatrix, 1, 0, Beta1.y);
57  gsl_matrix_set(pBetaMatrix, 2, 0, Beta1.z);
58  gsl_matrix_set(pBetaMatrix, 0, 1, Beta2.x);
59  gsl_matrix_set(pBetaMatrix, 1, 1, Beta2.y);
60  gsl_matrix_set(pBetaMatrix, 2, 1, Beta2.z);
61  gsl_matrix_set(pBetaMatrix, 0, 2, Beta3.x);
62  gsl_matrix_set(pBetaMatrix, 1, 2, Beta3.y);
63  gsl_matrix_set(pBetaMatrix, 2, 2, Beta3.z);
64  Dump3x3("BetaMatrix", pBetaMatrix);
65 
66  // Use Markley's singular value decomposition (SVD) method
67  // A detailed description can be found here
68  // http://www.control.auc.dk/~tb/best/aug23-Bak-svdalg.pdf
69 
70  // 1. Transpose the alpha matrix
71  GslRetcode = gsl_matrix_transpose(pAlphaMatrix);
72 
73  // 2. Compute the first intermediate matrix
74  gsl_matrix *pIntermediateMatrix1 = gsl_matrix_alloc(3, 3);
75  MatrixMatrixMultiply(pBetaMatrix, pAlphaMatrix, pIntermediateMatrix1);
76 
77  // 3. Compute the singular value decomoposition of the intermediate matrix
78  gsl_matrix *pV = gsl_matrix_alloc(3, 3);
79  gsl_vector *pS = gsl_vector_alloc(3);
80  gsl_vector *pWork = gsl_vector_alloc(3);
81  GslRetcode = gsl_linalg_SV_decomp(pIntermediateMatrix1, pV, pS, pWork);
82  // The intermediate matrix now contains the U matrix
83  // The V matrix is untransposed
84 
85  // 4. Compute the diagonal matrix
86  gsl_matrix *pDiagonal = gsl_matrix_calloc(3, 3);
87  gsl_matrix_set(pDiagonal, 0, 0, 1);
88  gsl_matrix_set(pDiagonal, 1, 1, 1);
89  gsl_matrix_set(pDiagonal, 2, 2, Matrix3x3Determinant(pIntermediateMatrix1) * Matrix3x3Determinant(pV));
90 
91  // 5. Compute the transform
92  gsl_matrix_transpose(pV);
93  gsl_matrix *pIntermediateMatrix2 = gsl_matrix_alloc(3, 3);
94  MatrixMatrixMultiply(pIntermediateMatrix1, pDiagonal, pIntermediateMatrix2);
95  MatrixMatrixMultiply(pIntermediateMatrix2, pV, pAlphaToBeta);
96 
97  Dump3x3("AlphaToBeta", pAlphaToBeta);
98 
99  if (nullptr != pBetaToAlpha)
100  {
101  // Invert the matrix to get the Apparent to Actual transform
102  if (!MatrixInvert3x3(pAlphaToBeta, pBetaToAlpha))
103  {
104  // pAlphaToBeta is singular and therefore is not a true transform
105  // and cannot be inverted. This probably means it contains at least
106  // one row or column that contains only zeroes
107  gsl_matrix_set_identity(pBetaToAlpha);
108  ASSDEBUG("CalculateTransformMatrices - AlphaToBeta matrix is singular!");
109  IDMessage(nullptr,
110  "Calculated Celestial to Telescope transformation matrix is singular (not a true transform).");
111  }
112 
113  Dump3x3("BetaToAlpha", pBetaToAlpha);
114  }
115 
116  // Clean up
117  gsl_matrix_free(pIntermediateMatrix1);
118  gsl_matrix_free(pV);
119  gsl_vector_free(pS);
120  gsl_vector_free(pWork);
121  gsl_matrix_free(pDiagonal);
122  gsl_matrix_free(pIntermediateMatrix2);
123  gsl_matrix_free(pBetaMatrix);
124  gsl_matrix_free(pAlphaMatrix);
125 }
126 
127 } // namespace AlignmentSubsystem
128 } // namespace INDI
INDI::AlignmentSubsystem::GetDisplayName
const char * GetDisplayName()
Definition: DummyMathPlugin.cpp:23
SVDMathPlugin.h
ASSDEBUG
#define ASSDEBUG(msg)
Definition: DriverCommon.h:17
DriverCommon.h
INDI::AlignmentSubsystem::Destroy
void Destroy(DummyMathPlugin *pPlugin)
Definition: DummyMathPlugin.cpp:18
INDI::AlignmentSubsystem::BasicMathPlugin::Matrix3x3Determinant
double Matrix3x3Determinant(gsl_matrix *pMatrix)
Caluclate the determinant of the supplied matrix.
Definition: BasicMathPlugin.cpp:835
IDMessage
void IDMessage(const char *dev, const char *msg,...) ATTRIBUTE_FORMAT_PRINTF(2
Function Drivers call to send log messages to Clients.
INDI::AlignmentSubsystem::BasicMathPlugin::Dump3x3
void Dump3x3(const char *Label, gsl_matrix *pMatrix)
Print out a 3x3 matrix to debug.
Definition: BasicMathPlugin.cpp:823
INDI::AlignmentSubsystem::SVDMathPlugin
This class implements the SVD math plugin.
Definition: SVDMathPlugin.h:20
INDI::AlignmentSubsystem::BasicMathPlugin::MatrixInvert3x3
bool MatrixInvert3x3(gsl_matrix *pInput, gsl_matrix *pInversion)
Calculate the inverse of the supplied matrix.
Definition: BasicMathPlugin.cpp:855
INDI::AlignmentSubsystem::Create
DummyMathPlugin * Create()
Definition: DummyMathPlugin.cpp:13
INDI
Namespace to encapsulate INDI client, drivers, and mediator classes.
Definition: AlignmentSubsystemForClients.cpp:11
INDI::AlignmentSubsystem::BasicMathPlugin::MatrixMatrixMultiply
void MatrixMatrixMultiply(gsl_matrix *pA, gsl_matrix *pB, gsl_matrix *pC)
Multiply matrix A by matrix B and put the result in C.
Definition: BasicMathPlugin.cpp:882