void zap (int *p, int *p1, int *p2, unsigned long k)
{
int i;
time_t t;
t = time (NULL);
srand (t);
for (i = 0; i < k; i++)
{
p2[i] = p1[i] = p[i] = slu (POTOLOK);
}
}
template<typename Scalar> void sparse_lu(int rows, int cols)
{
double density = std::max(8./(rows*cols), 0.01);
typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
typedef Matrix<Scalar,Dynamic,1> DenseVector;
DenseVector vec1 = DenseVector::Random(rows);
std::vector<Vector2i> zeroCoords;
std::vector<Vector2i> nonzeroCoords;
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2(rows, cols);
DenseVector b = DenseVector::Random(cols);
DenseVector refX(cols), x(cols);
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag, &zeroCoords, &nonzeroCoords);
FullPivLU<DenseMatrix> refLu(refMat2);
refX = refLu.solve(b);
#if defined(EIGEN_SUPERLU_SUPPORT) || defined(EIGEN_UMFPACK_SUPPORT)
Scalar refDet = refLu.determinant();
#endif
x.setZero();
// // SparseLU<SparseMatrix<Scalar> > (m2).solve(b,&x);
// // VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: default");
#ifdef EIGEN_UMFPACK_SUPPORT
{
// check solve
x.setZero();
SparseLU<SparseMatrix<Scalar>,UmfPack> lu(m2);
VERIFY(lu.succeeded() && "umfpack LU decomposition failed");
VERIFY(lu.solve(b,&x) && "umfpack LU solving failed");
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: umfpack");
VERIFY_IS_APPROX(refDet,lu.determinant());
// TODO check the extracted data
//std::cerr << slu.matrixL() << "\n";
}
#endif
#ifdef EIGEN_SUPERLU_SUPPORT
{
x.setZero();
SparseLU<SparseMatrix<Scalar>,SuperLU> slu(m2);
if (slu.succeeded())
{
if (slu.solve(b,&x)) {
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: SuperLU");
}
// std::cerr << refDet << " == " << slu.determinant() << "\n";
if (slu.solve(b, &x, SvTranspose)) {
VERIFY(b.isApprox(m2.transpose() * x, test_precision<Scalar>()));
}
if (slu.solve(b, &x, SvAdjoint)) {
VERIFY(b.isApprox(m2.adjoint() * x, test_precision<Scalar>()));
}
if (!NumTraits<Scalar>::IsComplex) {
VERIFY_IS_APPROX(refDet,slu.determinant()); // FIXME det is not very stable for complex
}
}
}
#endif
}
template<typename Scalar> void sparse_solvers(int rows, int cols)
{
double density = std::max(8./(rows*cols), 0.01);
typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
typedef Matrix<Scalar,Dynamic,1> DenseVector;
// Scalar eps = 1e-6;
DenseVector vec1 = DenseVector::Random(rows);
std::vector<Vector2i> zeroCoords;
std::vector<Vector2i> nonzeroCoords;
// test triangular solver
{
DenseVector vec2 = vec1, vec3 = vec1;
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols);
// lower
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.template marked<LowerTriangular>().solveTriangular(vec2),
m2.template marked<LowerTriangular>().solveTriangular(vec3));
// lower - transpose
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.template marked<LowerTriangular>().transpose().solveTriangular(vec2),
m2.template marked<LowerTriangular>().transpose().solveTriangular(vec3));
// upper
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.template marked<UpperTriangular>().solveTriangular(vec2),
m2.template marked<UpperTriangular>().solveTriangular(vec3));
// upper - transpose
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.template marked<UpperTriangular>().transpose().solveTriangular(vec2),
m2.template marked<UpperTriangular>().transpose().solveTriangular(vec3));
}
// test LLT
{
// TODO fix the issue with complex (see SparseLLT::solveInPlace)
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2(rows, cols);
DenseVector b = DenseVector::Random(cols);
DenseVector refX(cols), x(cols);
initSPD(density, refMat2, m2);
refMat2.llt().solve(b, &refX);
typedef SparseMatrix<Scalar,LowerTriangular|SelfAdjoint> SparseSelfAdjointMatrix;
if (!NumTraits<Scalar>::IsComplex)
{
x = b;
SparseLLT<SparseSelfAdjointMatrix> (m2).solveInPlace(x);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LLT: default");
}
#ifdef EIGEN_CHOLMOD_SUPPORT
x = b;
SparseLLT<SparseSelfAdjointMatrix,Cholmod>(m2).solveInPlace(x);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod");
#endif
if (!NumTraits<Scalar>::IsComplex)
{
#ifdef EIGEN_TAUCS_SUPPORT
x = b;
SparseLLT<SparseSelfAdjointMatrix,Taucs>(m2,IncompleteFactorization).solveInPlace(x);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LLT: taucs (IncompleteFactorization)");
x = b;
SparseLLT<SparseSelfAdjointMatrix,Taucs>(m2,SupernodalMultifrontal).solveInPlace(x);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LLT: taucs (SupernodalMultifrontal)");
x = b;
SparseLLT<SparseSelfAdjointMatrix,Taucs>(m2,SupernodalLeftLooking).solveInPlace(x);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LLT: taucs (SupernodalLeftLooking)");
#endif
}
}
// test LDLT
if (!NumTraits<Scalar>::IsComplex)
{
// TODO fix the issue with complex (see SparseLDLT::solveInPlace)
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2(rows, cols);
DenseVector b = DenseVector::Random(cols);
DenseVector refX(cols), x(cols);
//initSPD(density, refMat2, m2);
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, 0, 0);
refMat2 += refMat2.adjoint();
refMat2.diagonal() *= 0.5;
refMat2.ldlt().solve(b, &refX);
typedef SparseMatrix<Scalar,UpperTriangular|SelfAdjoint> SparseSelfAdjointMatrix;
x = b;
SparseLDLT<SparseSelfAdjointMatrix> ldlt(m2);
if (ldlt.succeeded())
ldlt.solveInPlace(x);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: default");
}
// test LU
{
static int count = 0;
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2(rows, cols);
DenseVector b = DenseVector::Random(cols);
DenseVector refX(cols), x(cols);
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag, &zeroCoords, &nonzeroCoords);
LU<DenseMatrix> refLu(refMat2);
refLu.solve(b, &refX);
#if defined(EIGEN_SUPERLU_SUPPORT) || defined(EIGEN_UMFPACK_SUPPORT)
Scalar refDet = refLu.determinant();
#endif
x.setZero();
// // SparseLU<SparseMatrix<Scalar> > (m2).solve(b,&x);
// // VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: default");
#ifdef EIGEN_SUPERLU_SUPPORT
{
x.setZero();
SparseLU<SparseMatrix<Scalar>,SuperLU> slu(m2);
if (slu.succeeded())
{
if (slu.solve(b,&x)) {
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: SuperLU");
}
// std::cerr << refDet << " == " << slu.determinant() << "\n";
if (count==0) {
VERIFY_IS_APPROX(refDet,slu.determinant()); // FIXME det is not very stable for complex
}
}
}
#endif
#ifdef EIGEN_UMFPACK_SUPPORT
{
// check solve
x.setZero();
SparseLU<SparseMatrix<Scalar>,UmfPack> slu(m2);
if (slu.succeeded()) {
if (slu.solve(b,&x)) {
if (count==0) {
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: umfpack"); // FIXME solve is not very stable for complex
}
}
VERIFY_IS_APPROX(refDet,slu.determinant());
// TODO check the extracted data
//std::cerr << slu.matrixL() << "\n";
}
}
#endif
count++;
}
}
void ScaleMaps::run(const cv::Mat& img, const vector<FeatureType>& features,
vector<float>& scaleMap)
{
size_t r, c, i, j, nr, nc, min_row, max_row, min_col, max_col;
size_t neighborCount, coefficientCount = 0;
std::vector<Eigen::Triplet<float>> coefficients; // list of non-zeros coefficients
vector<float> weights(9);
float m, v, sum;
size_t pixels = img.total();
int index = -1, nindex;
// Initialization
vector<bool> scale(pixels, false);
for (i = 0; i < features.size(); ++i)
{
const FeatureType& feat = features[i];
scale[((int)feat.y)*img.cols + (int)feat.x] = true;
}
// Adjust image values
cv::Mat scaledImg = img.clone();
scaledImg += 1;
scaledImg *= (1 / 32.0f);
// For each pixel in the image
for (r = 0; r < scaledImg.rows; ++r)
{
min_row = (size_t)std::max(int(r - 1), 0);
max_row = (size_t)std::min(scaledImg.rows - 1, int(r + 1));
for (c = 0; c < scaledImg.cols; ++c)
{
// Increment pixel index
++index;
// If this is not a feature point
if (!scale[index])
{
min_col = (size_t)std::max(int(c - 1), 0);
max_col = (size_t)std::min(scaledImg.cols - 1, int(c + 1));
neighborCount = 0;
// Loop over 3x3 neighborhoods matrix
// and calculate the variance of the intensities
for (nr = min_row; nr <= max_row; ++nr)
{
for (nc = min_col; nc <= max_col; ++nc)
{
if (nr == r && nc == c) continue;
weights[neighborCount++] = scaledImg.at<float>(nr, nc);
}
}
weights[neighborCount] = scaledImg.at<float>(r, c);
// Calculate the weights statistics
getStat(weights, neighborCount + 1, m, v);
m *= 0.6;
if (v < EPS) v = EPS; // Avoid division by 0
// Apply weight function
mWeightFunc(weights, neighborCount, scaledImg.at<float>(r, c), m, v);
// Normalize the weights and set to coefficients
sum = std::accumulate(weights.begin(), weights.begin() + neighborCount, 0.0f);
i = 0;
for (nr = min_row; nr <= max_row; ++nr)
{
for (nc = min_col; nc <= max_col; ++nc)
{
if (nr == r && nc == c) continue;
nindex = nr*scaledImg.cols + nc;
coefficients.push_back(Eigen::Triplet<float>(
index, nindex, -weights[i++] / sum));
}
}
}
// Add center coefficient
coefficients.push_back(Eigen::Triplet<float>(index, index, 1));
}
}
// Build right side equation vector
Eigen::VectorXf b = Eigen::VectorXf::Zero(pixels);
for (i = 0; i < features.size(); ++i)
{
const FeatureType& feat = features[i];
b[((int)feat.y)*scaledImg.cols + (int)feat.x] = feat.scale;
}
// Build left side equation matrix
Eigen::SparseMatrix<float> A(pixels, pixels);
A.setFromTriplets(coefficients.begin(), coefficients.end());
/*/// Debug ///
std::ofstream file("Output.m");
cv::Mat_<int> rows(1, coefficients.size()), cols(1, coefficients.size());
cv::Mat_<float> values(1, coefficients.size());
for (i = 0; i < coefficients.size(); ++i)
{
rows.at<int>(i) = coefficients[i].row();
cols.at<int>(i) = coefficients[i].col();
values.at<float>(i) = coefficients[i].value();
}
file << "cpp_rows = " << rows << ";" << std::endl;
file << "cpp_cols = " << cols << ";" << std::endl;
file << "cpp_values = " << values << ";" << std::endl;
/////////////*/
// Solving
Eigen::SparseLU<Eigen::SparseMatrix<float>> slu(A);
Eigen::VectorXf x = slu.solve(b);
// Copy to output
scaleMap.resize(pixels);
memcpy(scaleMap.data(), x.data(), pixels*sizeof(float));
}
