NumCpp  2.12.1
A Templatized Header Only C++ Implementation of the Python NumPy Library
wahbasProblem.hpp
Go to the documentation of this file.
1
33#pragma once
34
41#include "NumCpp/Linalg/det.hpp"
42#include "NumCpp/Linalg/svd.hpp"
43#include "NumCpp/NdArray.hpp"
44
45namespace nc::rotations
46{
47 //============================================================================
48 // Method Description:
61 template<typename dtype>
63 {
65
66 const auto wkShape = wk.shape();
67 if (wkShape.cols != 3)
68 {
69 THROW_INVALID_ARGUMENT_ERROR("wk matrix must be of shape [n, 3]");
70 }
71
72 const auto vkShape = vk.shape();
73 if (vkShape.cols != 3)
74 {
75 THROW_INVALID_ARGUMENT_ERROR("vk matrix must be of shape [n, 3]");
76 }
77
78 if (wkShape.rows != vkShape.rows)
79 {
80 THROW_INVALID_ARGUMENT_ERROR("wk and vk matrices must have the same number of rows");
81 }
82
83 if (ak.size() != wkShape.rows)
84 {
85 THROW_INVALID_ARGUMENT_ERROR("ak matrix must have the same number of elements as wk and vk rows");
86 }
87
88 auto b = zeros<dtype>(3, 3);
89 const auto cSlice = wk.cSlice();
90 for (uint32 row = 0; row < wkShape.rows; ++row)
91 {
92 const auto wkVec = wk(row, cSlice);
93 const auto vkVec = vk(row, cSlice);
94 b += ak[row] * dot(wkVec.transpose(), vkVec);
95 }
96
100
101 linalg::svd(b, u, s, vt);
102
103 auto m = eye<double>(3, 3);
104 m(0, 0) = 1.;
105 m(1, 1) = 1.;
106 m(2, 2) = linalg::det(u) * linalg::det(vt.transpose());
107
108 return dot(u, dot(m, vt));
109 }
110
111 //============================================================================
112 // Method Description:
124 template<typename dtype>
126 {
127 const auto ak = ones<dtype>({ 1, wk.shape().rows });
128 return wahbasProblem(wk, vk, ak);
129 }
130} // namespace nc::rotations
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:37
#define STATIC_ASSERT_ARITHMETIC(dtype)
Definition: StaticAsserts.hpp:39
size_type size() const noexcept
Definition: NdArrayCore.hpp:4524
self_type transpose() const
Definition: NdArrayCore.hpp:4882
const Shape & shape() const noexcept
Definition: NdArrayCore.hpp:4511
Slice cSlice(index_type inStartIdx=0, size_type inStepSize=1) const
Definition: NdArrayCore.hpp:1008
auto det(const NdArray< dtype > &inArray)
Definition: det.hpp:131
void svd(const NdArray< dtype > &inArray, NdArray< double > &outU, NdArray< double > &outS, NdArray< double > &outVt)
Definition: svd.hpp:51
Definition: DCM.hpp:39
NdArray< double > wahbasProblem(const NdArray< dtype > &wk, const NdArray< dtype > &vk, const NdArray< dtype > &ak)
Definition: wahbasProblem.hpp:62
NdArray< dtype > dot(const NdArray< dtype > &inArray1, const NdArray< dtype > &inArray2)
Definition: dot.hpp:47
std::uint32_t uint32
Definition: Types.hpp:40