sparse quadratic form

PX4 · Aug 26, 2020 · 13f092a · 13f092a
1 parent 3a5bfb2
commit 13f092a
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 0 deletions.
diff --git a/matrix/SparseVector.hpp b/matrix/SparseVector.hpp
@@ -166,6 +166,20 @@ matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M>& mat, const m
     return res;
 }
 
+// returns x.T * A * x
+template<typename Type, size_t M, size_t ... Idxs>
+Type quadraticForm(const matrix::SquareMatrix<Type, M>& A, const matrix::SparseVector<Type, M, Idxs...>& x) {
+    Type res = Type(0);
+    for (size_t i = 0; i < x.non_zeros(); i++) {
+        Type tmp = Type(0);
+        for (size_t j = 0; j < x.non_zeros(); j++) {
+            tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
+        }
+        res += x.atCompressedIndex(i) * tmp;
+    }
+    return res;
+}
+
 template<typename Type,size_t M, size_t... Idxs>
 constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N];
 

diff --git a/test/sparseVector.cpp b/test/sparseVector.cpp
@@ -91,6 +91,17 @@ TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
     EXPECT_TRUE(isEqual(res_dense, res_sparse));
 }
 
+TEST(sparseVectorTest, quadraticForm) {
+    float matrix_data[9] = {1, 2, 3,
+                            2, 4, 5,
+                            3, 5, 6
+                           };
+    const SquareMatrix<float, 3> dense_matrix(matrix_data);
+    const Vector3f dense_vec(0.f, 1.f, 5.f);
+    const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
+    EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
+}
+
 TEST(sparseVectorTest, norms) {
     const float data[2] = {3.f, 4.f};
     const SparseVectorf<4, 1, 3> sparse_vec(data);