wahbasProblem.hpp

Go to the documentation of this file.

#pragma once
 
#include "NumCpp/Core/Internal/Error.hpp"
#include "NumCpp/Core/Internal/StaticAsserts.hpp"
#include "NumCpp/Functions/dot.hpp"
#include "NumCpp/Functions/eye.hpp"
#include "NumCpp/Functions/ones.hpp"
#include "NumCpp/Functions/zeros.hpp"
#include "NumCpp/Linalg/det.hpp"
#include "NumCpp/Linalg/svd.hpp"
#include "NumCpp/NdArray.hpp"
 
namespace nc::rotations
{
    //============================================================================
    // Method Description:
    template<typename dtype>
    NdArray<double> wahbasProblem(const NdArray<dtype>& wk, const NdArray<dtype>& vk, const NdArray<dtype>& ak)
    {
        STATIC_ASSERT_ARITHMETIC(dtype);
 
        const auto wkShape = wk.shape();
        if (wkShape.cols != 3)
        {
            THROW_INVALID_ARGUMENT_ERROR("wk matrix must be of shape [n, 3]");
        }
 
        const auto vkShape = vk.shape();
        if (vkShape.cols != 3)
        {
            THROW_INVALID_ARGUMENT_ERROR("vk matrix must be of shape [n, 3]");
        }
 
        if (wkShape.rows != vkShape.rows)
        {
            THROW_INVALID_ARGUMENT_ERROR("wk and vk matrices must have the same number of rows");
        }
 
        if (ak.size() != wkShape.rows)
        {
            THROW_INVALID_ARGUMENT_ERROR("ak matrix must have the same number of elements as wk and vk rows");
        }
 
        auto       b      = zeros<dtype>(3, 3);
        const auto cSlice = wk.cSlice();
        for (uint32 row = 0; row < wkShape.rows; ++row)
        {
            const auto wkVec = wk(row, cSlice);
            const auto vkVec = vk(row, cSlice);
            b += ak[row] * dot(wkVec.transpose(), vkVec);
        }
 
        NdArray<double> u;
        NdArray<double> s;
        NdArray<double> vt;
 
        linalg::svd(b, u, s, vt);
 
        auto m  = eye<double>(3, 3);
        m(0, 0) = 1.;
        m(1, 1) = 1.;
        m(2, 2) = linalg::det(u) * linalg::det(vt.transpose());
 
        return dot(u, dot(m, vt));
    }
 
    //============================================================================
    // Method Description:
    template<typename dtype>
    NdArray<double> wahbasProblem(const NdArray<dtype>& wk, const NdArray<dtype>& vk)
    {
        const auto ak = ones<dtype>({ 1, wk.shape().rows });
        return wahbasProblem(wk, vk, ak);
    }
} // namespace nc::rotations