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);
	}
}
Example #2
0
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));
	}