NumCpp  2.1.0
A C++ implementation of the Python Numpy library
wahbasProblem.hpp
Go to the documentation of this file.
1 #pragma once
35 
38 #include "NumCpp/Functions/dot.hpp"
39 #include "NumCpp/Functions/eye.hpp"
42 #include "NumCpp/Linalg/det.hpp"
43 #include "NumCpp/Linalg/svd.hpp"
44 #include "NumCpp/NdArray.hpp"
45 
46 namespace nc
47 {
48  namespace rotations
49  {
50  //============================================================================
51  // Method Description:
64  template<typename dtype>
66  {
68 
69  const auto wkShape = wk.shape();
70  if (wkShape.cols != 3)
71  {
72  THROW_INVALID_ARGUMENT_ERROR("wk matrix must be of shape [n, 3]");
73  }
74 
75  const auto vkShape = vk.shape();
76  if (vkShape.cols != 3)
77  {
78  THROW_INVALID_ARGUMENT_ERROR("vk matrix must be of shape [n, 3]");
79  }
80 
81  if (wkShape.rows != vkShape.rows)
82  {
83  THROW_INVALID_ARGUMENT_ERROR("wk and vk matrices must have the same number of rows");
84  }
85 
86  if (ak.size() != wkShape.rows)
87  {
88  THROW_INVALID_ARGUMENT_ERROR("ak matrix must have the same number of elements as wk and vk rows");
89  }
90 
91  auto b = zeros<dtype>(3, 3);
92  const auto cSlice = wk.cSlice();
93  for (uint32 row = 0; row < wkShape.rows; ++row)
94  {
95  const auto wkVec = wk(row, cSlice);
96  const auto vkVec = vk(row, cSlice);
97  b += ak[row] * dot(wkVec.transpose(), vkVec);
98  }
99 
100  NdArray<double> u;
101  NdArray<double> s;
102  NdArray<double> vt;
103 
104  linalg::svd(b, u, s, vt);
105 
106  auto m = eye<double>(3, 3);
107  m(0, 0) = 1.0;
108  m(1, 1) = 1.0;
109  m(2, 2) = linalg::det(u) * linalg::det(vt.transpose());
110 
111  return dot(u, dot(m, vt));
112  }
113 
114  //============================================================================
115  // Method Description:
127  template<typename dtype>
129  {
130  const auto ak = ones<dtype>({1, wk.shape().rows});
131  return wahbasProblem(wk, vk, ak);
132  }
133  } // namespace rotations
134 } // namespace nc
StaticAsserts.hpp
nc::NdArray::shape
Shape shape() const noexcept
Definition: NdArrayCore.hpp:4312
zeros.hpp
Error.hpp
STATIC_ASSERT_ARITHMETIC
#define STATIC_ASSERT_ARITHMETIC(dtype)
Definition: StaticAsserts.hpp:38
nc::NdArray::transpose
NdArray< dtype > transpose() const
Definition: NdArrayCore.hpp:4608
nc::dot
NdArray< dtype > dot(const NdArray< dtype > &inArray1, const NdArray< dtype > &inArray2)
Definition: dot.hpp:48
nc::NdArray< double >
nc::uint32
std::uint32_t uint32
Definition: Types.hpp:41
nc::NdArray::cSlice
Slice cSlice(int32 inStartIdx=0, uint32 inStepSize=1) const noexcept
Definition: NdArrayCore.hpp:891
NdArray.hpp
dot.hpp
nc::linalg::svd
void svd(const NdArray< dtype > &inArray, NdArray< double > &outU, NdArray< double > &outS, NdArray< double > &outVt)
Definition: svd.hpp:54
nc::NdArray::size
size_type size() const noexcept
Definition: NdArrayCore.hpp:4326
nc
Definition: Coordinate.hpp:45
svd.hpp
det.hpp
THROW_INVALID_ARGUMENT_ERROR
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:37
eye.hpp
nc::linalg::det
dtype det(const NdArray< dtype > &inArray)
Definition: det.hpp:57
nc::rotations::wahbasProblem
NdArray< double > wahbasProblem(const NdArray< dtype > &wk, const NdArray< dtype > &vk, const NdArray< dtype > &ak)
Definition: wahbasProblem.hpp:65
ones.hpp