NumCpp  2.5.1
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
46 {
47  namespace rotations
48  {
49  //============================================================================
50  // Method Description:
63  template<typename dtype>
65  {
67 
68  const auto wkShape = wk.shape();
69  if (wkShape.cols != 3)
70  {
71  THROW_INVALID_ARGUMENT_ERROR("wk matrix must be of shape [n, 3]");
72  }
73 
74  const auto vkShape = vk.shape();
75  if (vkShape.cols != 3)
76  {
77  THROW_INVALID_ARGUMENT_ERROR("vk matrix must be of shape [n, 3]");
78  }
79 
80  if (wkShape.rows != vkShape.rows)
81  {
82  THROW_INVALID_ARGUMENT_ERROR("wk and vk matrices must have the same number of rows");
83  }
84 
85  if (ak.size() != wkShape.rows)
86  {
87  THROW_INVALID_ARGUMENT_ERROR("ak matrix must have the same number of elements as wk and vk rows");
88  }
89 
90  auto b = zeros<dtype>(3, 3);
91  const auto cSlice = wk.cSlice();
92  for (uint32 row = 0; row < wkShape.rows; ++row)
93  {
94  const auto wkVec = wk(row, cSlice);
95  const auto vkVec = vk(row, cSlice);
96  b += ak[row] * dot(wkVec.transpose(), vkVec);
97  }
98 
100  NdArray<double> s;
101  NdArray<double> vt;
102 
103  linalg::svd(b, u, s, vt);
104 
105  auto m = eye<double>(3, 3);
106  m(0, 0) = 1.0;
107  m(1, 1) = 1.0;
108  m(2, 2) = linalg::det(u) * linalg::det(vt.transpose());
109 
110  return dot(u, dot(m, vt));
111  }
112 
113  //============================================================================
114  // Method Description:
126  template<typename dtype>
128  {
129  const auto ak = ones<dtype>({1, wk.shape().rows});
130  return wahbasProblem(wk, vk, ak);
131  }
132  } // namespace rotations
133 } // namespace nc
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:36
#define STATIC_ASSERT_ARITHMETIC(dtype)
Definition: StaticAsserts.hpp:37
size_type size() const noexcept
Definition: NdArrayCore.hpp:4497
Slice cSlice(int32 inStartIdx=0, uint32 inStepSize=1) const noexcept
Definition: NdArrayCore.hpp:998
Shape shape() const noexcept
Definition: NdArrayCore.hpp:4483
NdArray< dtype > transpose() const
Definition: NdArrayCore.hpp:4830
dtype det(const NdArray< dtype > &inArray)
Definition: det.hpp:56
void svd(const NdArray< dtype > &inArray, NdArray< double > &outU, NdArray< double > &outS, NdArray< double > &outVt)
Definition: svd.hpp:53
NdArray< double > wahbasProblem(const NdArray< dtype > &wk, const NdArray< dtype > &vk, const NdArray< dtype > &ak)
Definition: wahbasProblem.hpp:64
Definition: Coordinate.hpp:45
NdArray< dtype > dot(const NdArray< dtype > &inArray1, const NdArray< dtype > &inArray2)
Definition: dot.hpp:47
std::uint32_t uint32
Definition: Types.hpp:40