1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html
4 
5 #include <iostream>
6 #include <unordered_map>
7 
8 #include "opencv2/core/base.hpp"
9 #include "opencv2/core/types.hpp"
10 #include "opencv2/core/utils/logger.hpp"
11 
12 #if defined(HAVE_EIGEN)
13 #include <Eigen/Core>
14 #include <Eigen/Sparse>
15 #include <Eigen/SparseCholesky>
16 
17 #include "opencv2/core/eigen.hpp"
18 #endif
19 
20 namespace cv
21 {
22 namespace kinfu
23 {
24 /*!
25  * \class BlockSparseMat
26  * Naive implementation of Sparse Block Matrix
27  */
28 template<typename _Tp, size_t blockM, size_t blockN>
29 struct BlockSparseMat
30 {
31     struct Point2iHash
32     {
operator ()cv::kinfu::BlockSparseMat::Point2iHash33         size_t operator()(const cv::Point2i& point) const noexcept
34         {
35             size_t seed                     = 0;
36             constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
37             seed ^= std::hash<int>()(point.x) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
38             seed ^= std::hash<int>()(point.y) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
39             return seed;
40         }
41     };
42     typedef Matx<_Tp, blockM, blockN> MatType;
43     typedef std::unordered_map<Point2i, MatType, Point2iHash> IDtoBlockValueMap;
44 
BlockSparseMatcv::kinfu::BlockSparseMat45     BlockSparseMat(size_t _nBlocks) : nBlocks(_nBlocks), ijValue() {}
46 
clearcv::kinfu::BlockSparseMat47     void clear()
48     {
49         ijValue.clear();
50     }
51 
refBlockcv::kinfu::BlockSparseMat52     inline MatType& refBlock(size_t i, size_t j)
53     {
54         Point2i p((int)i, (int)j);
55         auto it = ijValue.find(p);
56         if (it == ijValue.end())
57         {
58             it = ijValue.insert({ p, MatType::zeros() }).first;
59         }
60         return it->second;
61     }
62 
refElemcv::kinfu::BlockSparseMat63     inline _Tp& refElem(size_t i, size_t j)
64     {
65         Point2i ib((int)(i / blockM), (int)(j / blockN));
66         Point2i iv((int)(i % blockM), (int)(j % blockN));
67         return refBlock(ib.x, ib.y)(iv.x, iv.y);
68     }
69 
valBlockcv::kinfu::BlockSparseMat70     inline MatType valBlock(size_t i, size_t j) const
71     {
72         Point2i p((int)i, (int)j);
73         auto it = ijValue.find(p);
74         if (it == ijValue.end())
75             return MatType::zeros();
76         else
77             return it->second;
78     }
79 
valElemcv::kinfu::BlockSparseMat80     inline _Tp valElem(size_t i, size_t j) const
81     {
82         Point2i ib((int)(i / blockM), (int)(j / blockN));
83         Point2i iv((int)(i % blockM), (int)(j % blockN));
84         return valBlock(ib.x, ib.y)(iv.x, iv.y);
85     }
86 
diagonalcv::kinfu::BlockSparseMat87     Mat diagonal() const
88     {
89         // Diagonal max length is the number of columns in the sparse matrix
90         int diagLength = blockN * nBlocks;
91         cv::Mat diag   = cv::Mat::zeros(diagLength, 1, cv::DataType<_Tp>::type);
92 
93         for (int i = 0; i < diagLength; i++)
94         {
95             diag.at<_Tp>(i, 0) = valElem(i, i);
96         }
97         return diag;
98     }
99 
100 #if defined(HAVE_EIGEN)
toEigencv::kinfu::BlockSparseMat101     Eigen::SparseMatrix<_Tp> toEigen() const
102     {
103         std::vector<Eigen::Triplet<_Tp>> tripletList;
104         tripletList.reserve(ijValue.size() * blockM * blockN);
105         for (const auto& ijv : ijValue)
106         {
107             int xb = ijv.first.x, yb = ijv.first.y;
108             MatType vblock = ijv.second;
109             for (size_t i = 0; i < blockM; i++)
110             {
111                 for (size_t j = 0; j < blockN; j++)
112                 {
113                     _Tp val = vblock((int)i, (int)j);
114                     if (abs(val) >= NON_ZERO_VAL_THRESHOLD)
115                     {
116                         tripletList.push_back(Eigen::Triplet<_Tp>((int)(blockM * xb + i), (int)(blockN * yb + j), val));
117                     }
118                 }
119             }
120         }
121         Eigen::SparseMatrix<_Tp> EigenMat(blockM * nBlocks, blockN * nBlocks);
122         EigenMat.setFromTriplets(tripletList.begin(), tripletList.end());
123         EigenMat.makeCompressed();
124 
125         return EigenMat;
126     }
127 #endif
nonZeroBlockscv::kinfu::BlockSparseMat128     inline size_t nonZeroBlocks() const { return ijValue.size(); }
129 
operator +=cv::kinfu::BlockSparseMat130     BlockSparseMat<_Tp, blockM, blockN>& operator+=(const BlockSparseMat<_Tp, blockM, blockN>& other)
131     {
132         for (const auto& oijv : other.ijValue)
133         {
134             Point2i p = oijv.first;
135             MatType vblock = oijv.second;
136             this->refBlock(p.x, p.y) += vblock;
137         }
138 
139         return *this;
140     }
141 
142 #if defined(HAVE_EIGEN)
143     //! Function to solve a sparse linear system of equations HX = B
144     //! Requires Eigen
sparseSolvecv::kinfu::BlockSparseMat145     bool sparseSolve(InputArray B, OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) const
146     {
147         Eigen::SparseMatrix<_Tp> bigA = toEigen();
148         Mat mb = B.getMat().t();
149         Eigen::Matrix<_Tp, -1, 1> bigB;
150         cv2eigen(mb, bigB);
151 
152         Eigen::SparseMatrix<_Tp> bigAtranspose = bigA.transpose();
153         if(checkSymmetry && !bigA.isApprox(bigAtranspose))
154         {
155             CV_Error(Error::StsBadArg, "H matrix is not symmetrical");
156             return false;
157         }
158 
159         Eigen::SimplicialLDLT<Eigen::SparseMatrix<_Tp>> solver;
160 
161         solver.compute(bigA);
162         if (solver.info() != Eigen::Success)
163         {
164             CV_LOG_INFO(NULL, "Failed to eigen-decompose");
165             return false;
166         }
167         else
168         {
169             Eigen::Matrix<_Tp, -1, 1> solutionX = solver.solve(bigB);
170             if (solver.info() != Eigen::Success)
171             {
172                 CV_LOG_INFO(NULL, "Failed to eigen-solve");
173                 return false;
174             }
175             else
176             {
177                 eigen2cv(solutionX, X);
178                 if (predB.needed())
179                 {
180                     Eigen::Matrix<_Tp, -1, 1> predBEigen = bigA * solutionX;
181                     eigen2cv(predBEigen, predB);
182                 }
183                 return true;
184             }
185         }
186     }
187 #else
sparseSolvecv::kinfu::BlockSparseMat188     bool sparseSolve(InputArray /*B*/, OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) const
189     {
190         CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented");
191     }
192 #endif
193 
194     static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001);
195     size_t nBlocks;
196     IDtoBlockValueMap ijValue;
197 };
198 
199 }  // namespace kinfu
200 }  // namespace cv
201