21 template<
typename Type,
size_t M,
size_t N>
37 static_assert(M >= N,
"Matrix dimension should be M >= N");
42 for (
size_t j = 0; j < N; j++) {
44 for (
size_t i = j; i < M; i++) {
45 normx +=
_A(i,j) *
_A(i,j);
48 Type s =
_A(j,j) > 0 ? Type(-1) : Type(1);
49 Type u1 =
_A(j,j) - s*normx;
52 if (normx < Type(1e-8)) {
57 for (
size_t i = j+1; i < M; i++) {
58 w[i-j] =
_A(i,j) / u1;
62 _tau(j) = -s*u1/normx;
64 for (
size_t k = j+1; k < N; k++) {
66 for (
size_t i = j; i < M; i++) {
67 tmp += w[i-j] *
_A(i,k);
69 for (
size_t i = j; i < M; i++) {
70 _A(i,k) -=
_tau(j) * w[i-j] * tmp;
88 for (
size_t j = 0; j < N; j++) {
92 for (
size_t i = j+1; i < M; i++) {
96 for (
size_t i = j; i < M; i++) {
97 tmp += w[i-j] * qtbv(i);
100 for (
size_t i = j; i < M; i++) {
101 qtbv(i) -=
_tau(j) * w[i-j] * tmp;
120 for (
size_t i = N - 1; i < N; i--) {
121 printf(
"i %d\n", static_cast<int>(i));
123 for (
size_t r = i+1; r < N; r++) {
124 x(i) -=
_A(i,r) * x(r);
128 for (
size_t z = 0; z < N; z++) {
LeastSquaresSolver(const Matrix< Type, M, N > &A)
Class calculates QR decomposition which can be used for linear least squares.
Vector< Type, N > solve(const Vector< Type, M > &b)
Solve Ax=b for x.
Vector< Type, M > qtb(const Vector< Type, M > &b)
qtb Calculate Q^T * b
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)