SVDMathPlugin.cpp

Go to the documentation of this file.

 
 
#include "SVDMathPlugin.h"
 
#include "DriverCommon.h"
 
#include <gsl/gsl_linalg.h>
 
namespace INDI
{
namespace AlignmentSubsystem
{
#ifndef NO_PLUGIN_HOOKS
// Standard functions required for all plugins
extern "C" {
    SVDMathPlugin *Create()
    {
        return new SVDMathPlugin;
    }
 
    void Destroy(SVDMathPlugin *pPlugin)
    {
        delete pPlugin;
    }
 
    const char *GetDisplayName()
    {
        return "SVD Math Plugin";
    }
}
#endif
void SVDMathPlugin::CalculateTransformMatrices(const TelescopeDirectionVector &Alpha1,
        const TelescopeDirectionVector &Alpha2,
        const TelescopeDirectionVector &Alpha3,
        const TelescopeDirectionVector &Beta1,
        const TelescopeDirectionVector &Beta2,
        const TelescopeDirectionVector &Beta3, gsl_matrix *pAlphaToBeta,
        gsl_matrix *pBetaToAlpha)
{
    // Set up the column vectors
    gsl_matrix *pAlphaMatrix = gsl_matrix_alloc(3, 3);
    gsl_matrix_set(pAlphaMatrix, 0, 0, Alpha1.x);
    gsl_matrix_set(pAlphaMatrix, 1, 0, Alpha1.y);
    gsl_matrix_set(pAlphaMatrix, 2, 0, Alpha1.z);
    gsl_matrix_set(pAlphaMatrix, 0, 1, Alpha2.x);
    gsl_matrix_set(pAlphaMatrix, 1, 1, Alpha2.y);
    gsl_matrix_set(pAlphaMatrix, 2, 1, Alpha2.z);
    gsl_matrix_set(pAlphaMatrix, 0, 2, Alpha3.x);
    gsl_matrix_set(pAlphaMatrix, 1, 2, Alpha3.y);
    gsl_matrix_set(pAlphaMatrix, 2, 2, Alpha3.z);
    Dump3x3("AlphaMatrix", pAlphaMatrix);
    gsl_matrix *pBetaMatrix = gsl_matrix_alloc(3, 3);
    gsl_matrix_set(pBetaMatrix, 0, 0, Beta1.x);
    gsl_matrix_set(pBetaMatrix, 1, 0, Beta1.y);
    gsl_matrix_set(pBetaMatrix, 2, 0, Beta1.z);
    gsl_matrix_set(pBetaMatrix, 0, 1, Beta2.x);
    gsl_matrix_set(pBetaMatrix, 1, 1, Beta2.y);
    gsl_matrix_set(pBetaMatrix, 2, 1, Beta2.z);
    gsl_matrix_set(pBetaMatrix, 0, 2, Beta3.x);
    gsl_matrix_set(pBetaMatrix, 1, 2, Beta3.y);
    gsl_matrix_set(pBetaMatrix, 2, 2, Beta3.z);
    Dump3x3("BetaMatrix", pBetaMatrix);
 
    // Use Markley's singular value decomposition (SVD) method
    // A detailed description can be found here
    // http://www.control.auc.dk/~tb/best/aug23-Bak-svdalg.pdf
 
    // 1. Transpose the alpha matrix
    gsl_matrix_transpose(pAlphaMatrix);
 
    // 2. Compute the first intermediate matrix
    gsl_matrix *pIntermediateMatrix1 = gsl_matrix_alloc(3, 3);
    MatrixMatrixMultiply(pBetaMatrix, pAlphaMatrix, pIntermediateMatrix1);
 
    // 3. Compute the singular value decomposition of the intermediate matrix
    gsl_matrix *pV    = gsl_matrix_alloc(3, 3);
    gsl_vector *pS    = gsl_vector_alloc(3);
    gsl_vector *pWork = gsl_vector_alloc(3);
    gsl_linalg_SV_decomp(pIntermediateMatrix1, pV, pS, pWork);
    // The intermediate matrix now contains the U matrix
    // The V matrix is untransposed
 
    // 4. Compute the diagonal matrix
    gsl_matrix *pDiagonal = gsl_matrix_calloc(3, 3);
    gsl_matrix_set(pDiagonal, 0, 0, 1);
    gsl_matrix_set(pDiagonal, 1, 1, 1);
    gsl_matrix_set(pDiagonal, 2, 2, Matrix3x3Determinant(pIntermediateMatrix1) * Matrix3x3Determinant(pV));
 
    // 5. Compute the transform
    gsl_matrix_transpose(pV);
    gsl_matrix *pIntermediateMatrix2 = gsl_matrix_alloc(3, 3);
    MatrixMatrixMultiply(pIntermediateMatrix1, pDiagonal, pIntermediateMatrix2);
    MatrixMatrixMultiply(pIntermediateMatrix2, pV, pAlphaToBeta);
 
    Dump3x3("AlphaToBeta", pAlphaToBeta);
 
    if (nullptr != pBetaToAlpha)
    {
        // Invert the matrix to get the Apparent to Actual transform
        if (!MatrixInvert3x3(pAlphaToBeta, pBetaToAlpha))
        {
            // pAlphaToBeta is singular and therefore is not a true transform
            // and cannot be inverted. This probably means it contains at least
            // one row or column that contains only zeroes
            gsl_matrix_set_identity(pBetaToAlpha);
            ASSDEBUG("CalculateTransformMatrices - AlphaToBeta matrix is singular!");
            IDMessage(nullptr,
                      "Calculated Celestial to Telescope transformation matrix is singular (not a true transform).");
        }
 
        Dump3x3("BetaToAlpha", pBetaToAlpha);
    }
 
    // Clean up
    gsl_matrix_free(pIntermediateMatrix1);
    gsl_matrix_free(pV);
    gsl_vector_free(pS);
    gsl_vector_free(pWork);
    gsl_matrix_free(pDiagonal);
    gsl_matrix_free(pIntermediateMatrix2);
    gsl_matrix_free(pBetaMatrix);
    gsl_matrix_free(pAlphaMatrix);
}
 
} // namespace AlignmentSubsystem
} // namespace INDI