NumCpp  2.11.0
A Templatized Header Only C++ Implementation of the Python NumPy Library
wahbasProblem.hpp
Go to the documentation of this file.
1 #pragma once
34 
37 #include "NumCpp/Functions/dot.hpp"
38 #include "NumCpp/Functions/eye.hpp"
41 #include "NumCpp/Linalg/det.hpp"
42 #include "NumCpp/Linalg/svd.hpp"
43 #include "NumCpp/NdArray.hpp"
44 
45 namespace 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 
99  NdArray<double> vt;
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:4477
self_type transpose() const
Definition: NdArrayCore.hpp:4837
const Shape & shape() const noexcept
Definition: NdArrayCore.hpp:4464
Slice cSlice(index_type inStartIdx=0, size_type inStepSize=1) const
Definition: NdArrayCore.hpp:961
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