示例#1
0
void BSpline::updateControlPoints(const DenseMatrix &A)
{
    if (A.cols() != coefficients.rows() || A.cols() != knotaverages.rows())
        throw Exception("BSpline::updateControlPoints: Incompatible size of linear transformation matrix.");
    coefficients = A*coefficients;
    knotaverages = A*knotaverages;
}
	GPUDenseMatrixOperation(const DenseMatrix& matrix)
	{
		mat = viennacl::matrix<ScalarType>(matrix.cols(),matrix.rows());
		vec = viennacl::matrix<ScalarType>(matrix.cols(),1);
		res = viennacl::matrix<ScalarType>(matrix.cols(),1);
		viennacl::copy(matrix,mat);
	}
	GPUDenseImplicitSquareMatrixOperation(const DenseMatrix& matrix)
	{
		timed_context c("Storing matrices");
		mat = viennacl::matrix<ScalarType>(matrix.cols(),matrix.rows());
		vec = viennacl::matrix<ScalarType>(matrix.cols(),1);
		res = viennacl::matrix<ScalarType>(matrix.cols(),1);
		viennacl::copy(matrix,mat);
	}
示例#4
0
	EmbeddingResult embed(const MatrixType& wm, IndexType target_dimension, unsigned int skip)
	{
		timed_context context("Randomized eigendecomposition");
		
		DenseMatrix O(wm.rows(), target_dimension+skip);
		for (IndexType i=0; i<O.rows(); ++i)
		{
			IndexType j=0;
			for ( ; j+1 < O.cols(); j+= 2)
			{
				ScalarType v1 = (ScalarType)(rand()+1.f)/((float)RAND_MAX+2.f);
				ScalarType v2 = (ScalarType)(rand()+1.f)/((float)RAND_MAX+2.f);
				ScalarType len = sqrt(-2.f*log(v1));
				O(i,j) = len*cos(2.f*M_PI*v2);
				O(i,j+1) = len*sin(2.f*M_PI*v2);
			}
			for ( ; j < O.cols(); j++)
			{
				ScalarType v1 = (ScalarType)(rand()+1.f)/((float)RAND_MAX+2.f);
				ScalarType v2 = (ScalarType)(rand()+1.f)/((float)RAND_MAX+2.f);
				ScalarType len = sqrt(-2.f*log(v1));
				O(i,j) = len*cos(2.f*M_PI*v2);
			}
		}
		MatrixTypeOperation operation(wm);

		DenseMatrix Y = operation(O);
		for (IndexType i=0; i<Y.cols(); i++)
		{
			for (IndexType j=0; j<i; j++)
			{
				ScalarType r = Y.col(i).dot(Y.col(j));
				Y.col(i) -= r*Y.col(j);
			}
			ScalarType norm = Y.col(i).norm();
			if (norm < 1e-4)
			{
				for (int k = i; k<Y.cols(); k++)
					Y.col(k).setZero();
			}
			Y.col(i) *= (1.f / norm);
		}

		DenseMatrix B1 = operation(Y);
		DenseMatrix B = Y.householderQr().solve(B1);
		DenseSelfAdjointEigenSolver eigenOfB(B);

		if (eigenOfB.info() == Eigen::Success)
		{
			DenseMatrix embedding = (Y*eigenOfB.eigenvectors()).block(0, skip, wm.cols(), target_dimension);
			return EmbeddingResult(embedding,eigenOfB.eigenvalues());
		}
		else
		{
			throw eigendecomposition_error("eigendecomposition failed");
		}
		return EmbeddingResult();
	}
示例#5
0
		TapkeeOutput embedUsing(const DenseMatrix& matrix) const
		{
			vector<IndexType> indices(matrix.cols());
			for (IndexType i=0; i<matrix.cols(); i++) indices[i] = i;
			eigen_kernel_callback kcb(matrix);
			eigen_distance_callback dcb(matrix);
			eigen_features_callback fvcb(matrix);
			return tapkee::embed(indices.begin(),indices.end(),kcb,dcb,fvcb,parameters);
		}
示例#6
0
EigendecompositionResult eigendecomposition_impl_randomized(const MatrixType& wm, IndexType target_dimension, unsigned int skip)
{
	timed_context context("Randomized eigendecomposition");
	
	DenseMatrix O(wm.rows(), target_dimension+skip);
	for (IndexType i=0; i<O.rows(); ++i)
	{
		for (IndexType j=0; j<O.cols(); j++)
		{
			O(i,j) = tapkee::gaussian_random();
		}
	}
	MatrixOperationType operation(wm);

	DenseMatrix Y = operation(O);
	for (IndexType i=0; i<Y.cols(); i++)
	{
		for (IndexType j=0; j<i; j++)
		{
			ScalarType r = Y.col(i).dot(Y.col(j));
			Y.col(i) -= r*Y.col(j);
		}
		ScalarType norm = Y.col(i).norm();
		if (norm < 1e-4)
		{
			for (int k = i; k<Y.cols(); k++)
				Y.col(k).setZero();
		}
		Y.col(i) *= (1.f / norm);
	}

	DenseMatrix B1 = operation(Y);
	DenseMatrix B = Y.householderQr().solve(B1);
	DenseSelfAdjointEigenSolver eigenOfB(B);

	if (eigenOfB.info() == Eigen::Success)
	{
		if (MatrixOperationType::largest)
		{
			assert(skip==0);
			DenseMatrix selected_eigenvectors = (Y*eigenOfB.eigenvectors()).rightCols(target_dimension);
			return EigendecompositionResult(selected_eigenvectors,eigenOfB.eigenvalues());
		} 
		else
		{
			DenseMatrix selected_eigenvectors = (Y*eigenOfB.eigenvectors()).leftCols(target_dimension+skip).rightCols(target_dimension);
			return EigendecompositionResult(selected_eigenvectors,eigenOfB.eigenvalues());
		}
	}
	else
	{
		throw eigendecomposition_error("eigendecomposition failed");
	}
	return EigendecompositionResult();
}
示例#7
0
DenseMatrix<FloatType> DenseMatrix<FloatType>::subtract(const DenseMatrix<FloatType> &A, const DenseMatrix<FloatType> &B)
{
	MLIB_ASSERT_STR(A.rows() == B.rows() && A.cols() == B.cols(), "invalid matrix dimensions");

	const UINT rows = A.m_rows;
	const UINT cols = A.m_cols;

	DenseMatrix<FloatType> result(A.m_rows, A.m_cols);
	for(UINT row = 0; row < rows; row++)
		for(UINT col = 0; col < cols; col++)
			result.m_dataPtr[row * cols + col] = A.m_dataPtr[row * cols + col] - B.m_dataPtr[row * cols + col];
	return result;
}
ScalarType average_neighbor_distance(const DenseMatrix& data, const Neighbors& neighbors)
{
	IndexType k = neighbors[0].size();
	ScalarType average_distance = 0;

	for (IndexType i = 0; i < data.cols(); ++i)
	{
		for (IndexType j = 0; j < k; ++j)
		{
			average_distance += (data.col(i) - data.col(neighbors[i][j])).norm();
		}
	}
	return average_distance / (k * data.cols());
}
示例#9
0
/*
 * Calculate coefficients of B-spline representing a multivariate polynomial
 *
 * The polynomial f(x), with x in R^n, has m terms on the form
 * f(x) = c(0)*x(0)^E(0,0)*x(1)^E(0,1)*...*x(n-1)^E(0,n-1)
 *       +c(1)*x(0)^E(1,0)*x(1)^E(1,1)*...*x(n-1)^E(1,n-1)
 *       +...
 *       +c(m-1)*x(0)^E(m-1,0)*x(1)^E(m-1,1)*...*x(n-1)^E(m-1,n-1)
 * where c in R^m is a vector with coefficients for each of the m terms,
 * and E in N^(mxn) is a matrix with the exponents of each variable in each of the m terms,
 * e.g. the first row of E defines the first term with variable exponents E(0,0) to E(0,n-1).
 *
 * Note: E must be a matrix of nonnegative integers
 */
DenseMatrix getBSplineBasisCoefficients(DenseVector c, DenseMatrix E, std::vector<double> lb, std::vector<double> ub)
{
    unsigned int dim = E.cols();
    unsigned int terms = E.rows();
    assert(dim >= 1); // At least one variable
    assert(terms >= 1); // At least one term (assumes that c is a column vector)
    assert(terms == c.rows());
    assert(dim == lb.size());
    assert(dim == ub.size());

    // Get highest power of each variable
    DenseVector powers = E.colwise().maxCoeff();

    // Store in std vector
    std::vector<unsigned int> powers2;
    for (unsigned int i = 0; i < powers.size(); ++i)
        powers2.push_back(powers(i));

    // Calculate tensor product transformation matrix T
    DenseMatrix T = getTransformationMatrix(powers2, lb, ub);

    // Compute power basis coefficients (lambda vector)
    SparseMatrix L(T.cols(),1);
    L.setZero();

    for (unsigned int i = 0; i < terms; i++)
    {
        SparseMatrix Li(1,1);
        Li.insert(0,0) = 1;

        for (unsigned int j = 0; j < dim; j++)
        {
            int e = E(i,j);
            SparseVector li(powers(j)+1);
            li.reserve(1);
            li.insert(e) = 1;

            SparseMatrix temp = Li;
            Li = kroneckerProduct(temp, li);
        }

        L += c(i)*Li;
    }

    // Compute B-spline coefficients
    DenseMatrix C = T*L;

    return C;
}
示例#10
0
DenseVector ConstraintBSpline::evalJacobian(const DenseVector &x) const
{
    DenseVector xa = adjustToDomainBounds(x);
    DenseVector dx = DenseVector::Zero(nnzJacobian);

    //return centralDifference(xa);

    // Get x-values
    DenseVector xx = xa.block(0,0,bspline.getNumVariables(),1);

    // Evaluate B-spline Jacobian
    DenseMatrix jac = bspline.evalJacobian(xx);

    // Derivatives on inputs x
    int k = 0;
    for (int i = 0; i < jac.rows(); i++)
    {
        for (int j = 0; j < jac.cols(); j++)
        {
            dx(k++) = jac(i,j);
        }
    }

    // Derivatives on outputs y
    for (unsigned int i = 0; i < numConstraints; i++)
    {
        dx(k++) = -1;
    }

    return dx;
}
示例#11
0
void BSpline::setControlPoints(const DenseMatrix &controlPoints)
{
    if (controlPoints.cols() != numVariables + 1)
        throw Exception("BSpline::setControlPoints: Incompatible size of control point matrix.");

    int nc = controlPoints.rows();

    knotaverages = controlPoints.block(0, 0, nc, numVariables);
    coefficients = controlPoints.block(0, numVariables, nc, 1);

    checkControlPoints();
}
示例#12
0
DenseMatrix<FloatType> DenseMatrix<FloatType>::multiply(const DenseMatrix<FloatType> &A, const DenseMatrix<FloatType> &B)
{
	MLIB_ASSERT_STR(A.cols() == B.rows(), "invalid dimensions");

	const UINT rows = A.rows();
	const UINT cols = B.cols();
	const UINT innerCount = A.cols();

	DenseMatrix<FloatType> result(rows, cols);
	
	for(UINT row = 0; row < rows; row++)
		for(UINT col = 0; col < cols; col++)
		{
			FloatType sum = 0.0;
			for(UINT inner = 0; inner < innerCount; inner++)
				sum += A(row, inner) * B(inner, col);
			result(row, col) = sum;
		}

	return result;
}
SparseMatrixNeighborsPair angles_matrix_and_neighbors(const Neighbors& neighbors,
                                                      const DenseMatrix& data)
{
	const IndexType k = neighbors[0].size();
	const IndexType n_vectors = data.cols();

	SparseTriplets sparse_triplets;
	sparse_triplets.reserve(k * n_vectors);
	/* I tried to find better naming, but... */
	Neighbors most_collinear_neighbors_of_neighbors;
	most_collinear_neighbors_of_neighbors.reserve(n_vectors);

	for (IndexType i = 0; i < n_vectors; ++i)
	{
		const LocalNeighbors& current_neighbors = neighbors[i];
		LocalNeighbors most_collinear_current_neighbors;
		most_collinear_current_neighbors.reserve(k);

		for (IndexType j = 0; j < k; ++j)
		{
			const LocalNeighbors& neighbors_of_neighbor = neighbors[current_neighbors[j]];
			/* The closer the cos value to -1.0 - the closer the angle to 180.0 */
			ScalarType min_cos_value = 1.0, current_cos_value;
			/* This value will be updated during the seach for most collinear neighbor */
			most_collinear_current_neighbors.push_back(0);

			for (IndexType l = 0; l < k; ++l)
			{
				DenseVector neighbor_to_point = data.col(i) - data.col(current_neighbors[j]);
				DenseVector neighbor_to_its_neighbor = data.col(neighbors_of_neighbor[l])
														- data.col(current_neighbors[j]);
				current_cos_value = neighbor_to_point.dot(neighbor_to_its_neighbor) /
									(neighbor_to_point.norm() *
									 neighbor_to_its_neighbor.norm());
				if (current_cos_value < min_cos_value)
				{
					most_collinear_current_neighbors[j] = neighbors_of_neighbor[l];
					min_cos_value = current_cos_value;
				}
			}

			SparseTriplet triplet(i, most_collinear_current_neighbors[j], min_cos_value);
			sparse_triplets.push_back(triplet);
		}
		most_collinear_neighbors_of_neighbors.push_back(most_collinear_current_neighbors);
	}
	return SparseMatrixNeighborsPair
		(sparse_matrix_from_triplets(sparse_triplets, n_vectors, n_vectors),
		 most_collinear_neighbors_of_neighbors);
}
示例#14
0
std::vector<FloatType> DenseMatrix<FloatType>::multiply(const DenseMatrix<FloatType> &A, const std::vector<FloatType> &B)
{
	MLIB_ASSERT_STR(A.cols() == B.size(), "invalid dimensions");
	const int rows = A.m_rows;
	const UINT cols = A.m_cols;
	std::vector<FloatType> result(rows);
//#ifdef MLIB_OPENMP
//#pragma omp parallel for
//#endif
	for(int row = 0; row < rows; row++)
	{
		FloatType val = 0.0;
		for(UINT col = 0; col < cols; col++)
			val += A.m_dataPtr[row * cols + col] * B[col];
		result[row] = val;
	}
	return result;
}
示例#15
0
文件: redsvd.hpp 项目: dblalock/dig
		void compute(const MatrixType& A, const Index rank)
		{
			if(A.cols() == 0 || A.rows() == 0)
				return;

			Index r = (rank < A.cols()) ? rank : A.cols();

			r = (r < A.rows()) ? r : A.rows();

			// Gaussian Random Matrix for A^T
			DenseMatrix O(A.rows(), r);
			sample_gaussian(O);

			// Compute Sample Matrix of A^T
			DenseMatrix Y = A.transpose() * O;

			// Orthonormalize Y
			gram_schmidt(Y);

			// Range(B) = Range(A^T)
			DenseMatrix B = A * Y;

			// Gaussian Random Matrix
			DenseMatrix P(B.cols(), r);
			sample_gaussian(P);

			// Compute Sample Matrix of B
			DenseMatrix Z = B * P;

			// Orthonormalize Z
			gram_schmidt(Z);

			// Range(C) = Range(B)
			DenseMatrix C = Z.transpose() * B;

			Eigen::JacobiSVD<DenseMatrix> svdOfC(C, Eigen::ComputeThinU | Eigen::ComputeThinV);

			// C = USV^T
			// A = Z * U * S * V^T * Y^T()
			m_matrixU = Z * svdOfC.matrixU();
			m_vectorS = svdOfC.singularValues();
			m_matrixV = Y * svdOfC.matrixV();
		}
示例#16
0
文件: pca.hpp 项目: Argram/shogun
DenseMatrix project(const DenseMatrix& projection_matrix, const DenseVector& mean_vector,
                    RandomAccessIterator begin, RandomAccessIterator end, 
                    FeatureVectorCallback callback, IndexType dimension)
{
	timed_context context("Data projection");

	DenseVector current_vector(dimension);
	DenseVector current_vector_subtracted_mean(dimension);

	DenseMatrix embedding = DenseMatrix::Zero((end-begin),projection_matrix.cols());

	for (RandomAccessIterator iter=begin; iter!=end; ++iter)
	{
		callback(*iter,current_vector);
		current_vector_subtracted_mean = current_vector - mean_vector;
		embedding.row(iter-begin) = projection_matrix.transpose()*current_vector_subtracted_mean;
	}

	return embedding;
}
void manifold_sculpting_embed(RandomAccessIterator begin, RandomAccessIterator end,
                              DenseMatrix& data, IndexType target_dimension,
                              const Neighbors& neighbors, DistanceCallback callback,
                              IndexType max_iteration, ScalarType squishing_rate)
{
	/* Step 1: Get initial distances to each neighbor and initial
	 * angles between the point Pi, each neighbor Nij, and the most
	 * collinear neighbor of Nij.
	 */
	ScalarType initial_average_distance;
	SparseMatrix distances_to_neighbors =
		neighbors_distances_matrix(begin, end, neighbors, callback, initial_average_distance);
	SparseMatrixNeighborsPair angles_and_neighbors =
		angles_matrix_and_neighbors(neighbors, data);

	/* Step 2: Optionally preprocess the data using PCA
	 * (skipped for now).
	 */
	ScalarType no_improvement_counter = 0, normal_counter = 0;
	ScalarType current_multiplier = squishing_rate;
	ScalarType learning_rate = initial_average_distance;
	ScalarType best_error = DBL_MAX, current_error, point_error;
	/* Step 3: Do until no improvement is made for some period
	 * (or until max_iteration number is reached):
	 */
	while (((no_improvement_counter++ < max_number_of_iterations_without_improvement)
			|| (current_multiplier >  multiplier_treshold))
			&& (normal_counter++ < max_iteration))
	{
		/* Step 3a: Scale the data in non-preserved dimensions
		 * by a factor of squishing_rate.
		 */
		data.bottomRows(data.rows() - target_dimension) *= squishing_rate;
		while (average_neighbor_distance(data, neighbors) < initial_average_distance)
		{
			data.topRows(target_dimension) /= squishing_rate;
		}
		current_multiplier *= squishing_rate;

		/* Step 3b: Restore the previously computed relationships
		 * (distances to neighbors and angles to ...) by adjusting
		 * data points in first target_dimension dimensions.
		 */
		/* Start adjusting from a random point */
		IndexType start_point_index = std::rand() % data.cols();
		std::deque<IndexType> points_to_adjust;
		points_to_adjust.push_back(start_point_index);
		ScalarType steps_made = 0;
		current_error = 0;
		std::set<IndexType> adjusted_points;

		while (!points_to_adjust.empty())
		{
			IndexType current_point_index = points_to_adjust.front();
			points_to_adjust.pop_front();
			if (adjusted_points.count(current_point_index) == 0)
			{
			DataForErrorFunc error_func_data = {
					distances_to_neighbors,
					angles_and_neighbors.first,
					neighbors,
					angles_and_neighbors.second,
					adjusted_points,
					initial_average_distance
				};
				adjust_point_at_index(current_point_index, data, target_dimension,
									learning_rate, error_func_data, point_error);
				current_error += point_error;
				/* Insert all neighbors into deque */
				std::copy(neighbors[current_point_index].begin(),
				          neighbors[current_point_index].end(),
				          std::back_inserter(points_to_adjust));
				adjusted_points.insert(current_point_index);
			}
		}

		if (steps_made > data.cols())
			learning_rate *= learning_rate_grow_factor;
		else
			learning_rate *= learning_rate_shrink_factor;

		if (current_error < best_error)
		{
			best_error = current_error;
			no_improvement_counter = 0;
		}
	}

	data.conservativeResize(target_dimension, Eigen::NoChange);
	data.transposeInPlace();
}
示例#18
0
bool ConstraintBSpline::controlPointBoundsDeduction() const
{
    // Get variable bounds
    auto xlb = bspline.getDomainLowerBound();
    auto xub = bspline.getDomainUpperBound();

    // Use these instead?
//    for (unsigned int i = 0; i < bspline.getNumVariables(); i++)
//    {
//        xlb.at(i) = variables.at(i)->getLowerBound();
//        xub.at(i) = variables.at(i)->getUpperBound();
//    }

    double lowerBound = variables.back()->getLowerBound(); // f(x) = y > lowerBound
    double upperBound = variables.back()->getUpperBound(); // f(x) = y < upperBound

    // Get knot vectors and basis degrees
    auto knotVectors = bspline.getKnotVectors();
    auto basisDegrees = bspline.getBasisDegrees();

    // Compute n value for each variable
    // Total number of control points is ns(0)*...*ns(d-1)
    std::vector<unsigned int> numBasisFunctions = bspline.getNumBasisFunctions();

    // Get matrix of coefficients
    DenseMatrix cps = controlPoints;
    DenseMatrix coeffs = cps.block(bspline.getNumVariables(), 0, 1, cps.cols());

    for (unsigned int d = 0; d < bspline.getNumVariables(); d++)
    {
        if (assertNear(xlb.at(d), xub.at(d)))
            continue;

        auto n = numBasisFunctions.at(d);
        auto p = basisDegrees.at(d);
        std::vector<double> knots = knotVectors.at(d);
        assert(knots.size() == n+p+1);

        // Tighten lower bound
        unsigned int i = 1;
        for (; i <= n; i++)
        {
            // Knot interval of interest: [t_0, t_i]

            // Selection matrix
            DenseMatrix S = DenseMatrix::Ones(1,1);

            for (unsigned int d2 = 0; d2 < bspline.getNumVariables(); d2++)
            {
                DenseMatrix temp(S);

                DenseMatrix Sd_full = DenseMatrix::Identity(numBasisFunctions.at(d2),numBasisFunctions.at(d2));
                DenseMatrix Sd(Sd_full);
                if (d == d2)
                    Sd = Sd_full.block(0,0,n,i);

                S = kroneckerProduct(temp, Sd);
            }

            // Control points that have support in [t_0, t_i]
            DenseMatrix selc = coeffs*S;
            DenseVector minCP = selc.rowwise().minCoeff();
            DenseVector maxCP = selc.rowwise().maxCoeff();
            double minv = minCP(0);
            double maxv = maxCP(0);

            // Investigate feasibility
            if (minv > upperBound || maxv < lowerBound)
                continue; // infeasible
            else
                break; // feasible
        }

        // New valid lower bound on x(d) is knots(i-1)
        if (i > 1)
        {
            if (!variables.at(d)->updateLowerBound(knots.at(i-1)))
                return false;
        }

        // Tighten upper bound
        i = 1;
        for (; i <= n; i++)
        {
            // Knot interval of interest: [t_{n+p-i}, t_{n+p}]

            // Selection matrix
            DenseMatrix S = DenseMatrix::Ones(1,1);

            for (unsigned int d2 = 0; d2 < bspline.getNumVariables(); d2++)
            {
                DenseMatrix temp(S);

                DenseMatrix Sd_full = DenseMatrix::Identity(numBasisFunctions.at(d2),numBasisFunctions.at(d2));
                DenseMatrix Sd(Sd_full);
                if (d == d2)
                    Sd = Sd_full.block(0,n-i,n,i);

                S = kroneckerProduct(temp, Sd);
            }

            // Control points that have support in [t_{n+p-i}, t_{n+p}]
            DenseMatrix selc = coeffs*S;
            DenseVector minCP = selc.rowwise().minCoeff();
            DenseVector maxCP = selc.rowwise().maxCoeff();
            double minv = minCP(0);
            double maxv = maxCP(0);

            // Investigate feasibility
            if (minv > upperBound || maxv < lowerBound)
                continue; // infeasible
            else
                break; // feasible
        }

        // New valid lower bound on x(d) is knots(n+p-(i-1))
        if (i > 1)
        {
            if (!variables.at(d)->updateUpperBound(knots.at(n+p-(i-1))))
                return false;
            // NOTE: the upper bound seems to not be tight! can we use knots.at(n+p-i)?
        }

    }

    return true;
}
示例#19
0
ConstraintQuadratic::ConstraintQuadratic(std::vector<VariablePtr> variables, DenseMatrix A, DenseMatrix b, double c, double lb, double ub)
    : Constraint(variables), A(A), b(b), c(c)
{
    assert(A.cols() == (int)variables.size());
    assert(A.rows() == b.rows());
    assert(b.cols() == 1);

    numConstraints = 1;

    this->lb.push_back(lb);
    this->ub.push_back(ub);

    jacobianCalculated = true;
    hessianCalculated = true;
    constraintLinear = false;
    constraintConvex = false;
    convexRelaxationAvailable = true;

    nnzJacobian = A.rows();
    nnzHessian = 0;

    constraintName = "Constraint Quadratic";

    //    // Check for parameters for NaN
    //    for (int i = 0; i < A.rows(); i++)
    //    {
    //        for (int j = 0; j < A.cols(); j++)
    //        {
    //            bool nanA = false;
    //            if (A(i,j) != A(i,j)) nanA = true;
    //            assert(nanA == false);
    //        }
    //        bool nanb = false;
    //        if (b(i) != b(i)) nanb = true;
    //        assert(nanb == false);
    //    }

    // Calculate and store Hessian
    H = A.transpose() + A;

    // H is symmetric so fill out lower left triangle only
    for (int row = 0; row < H.rows(); row++)
    {
        for (int col = 0; col <= row; col++)
        {
            if (H(row,col) != 0)
            {
                nnzHessian++;
            }
        }
    }

    // Check convexity using Hessian
    Eigen::EigenSolver<DenseMatrix> es(H);
    DenseVector eigs = es.eigenvalues().real();
    double minEigVal = 0;
    for (int i = 0; i < eigs.rows(); i++)
    {
        if (eigs(i) < minEigVal) minEigVal = eigs(i);
    }

    if (minEigVal >= 0 && lb <= -INF)
    {
        constraintConvex = true;
    }
    // Note could also check that max. eigen value <= 0 and ub = INF

    checkConstraintSanity();
}