some matlab changes and modified Guassian:QR to mimic Constrained::QR and deleted DenseQR related stuff
							parent
							
								
									6f2ef4ed72
								
							
						
					
					
						commit
						21e2be0ad6
					
				|  | @ -18,7 +18,6 @@ sources += Vector.cpp svdcmp.cpp Matrix.cpp | |||
| check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix  | ||||
| 
 | ||||
| if USE_LAPACK | ||||
| sources += DenseQR.cpp DenseQRUtil.cpp | ||||
| check_PROGRAMS += tests/testDenseQRUtil | ||||
| endif | ||||
| 
 | ||||
|  |  | |||
|  | @ -21,13 +21,8 @@ class GaussianFactor { | |||
| 			Matrix A3, | ||||
| 			Vector b_in, | ||||
| 			const SharedDiagonal& model); | ||||
| 	void print(string s) const; | ||||
| 	bool equals(const GaussianFactor& lf, double tol) const; | ||||
| 	bool empty() const; | ||||
| 	Vector get_b() const; | ||||
| 	Matrix get_A(string key) const; | ||||
| 	double error(const VectorValues& c) const; | ||||
| 	bool involves(string key) const; | ||||
| 	Matrix getA(string key) const; | ||||
| 	pair<Matrix,Vector> matrix(const Ordering& ordering) const; | ||||
| 	pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const; | ||||
| }; | ||||
|  |  | |||
							
								
								
									
										9
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										9
									
								
								gtsam.h
								
								
								
								
							|  | @ -17,11 +17,20 @@ class Ordering { | |||
| 
 | ||||
| class VectorValues { | ||||
|   VectorValues(); | ||||
|   VectorValues(size_t nVars, size_t varDim); | ||||
|   void print(string s) const; | ||||
|   bool equals(const VectorValues& expected, double tol) const; | ||||
|   size_t size() const; | ||||
| }; | ||||
| 
 | ||||
| class GaussianFactor { | ||||
| 	void print(string s) const; | ||||
| 	bool equals(const GaussianFactor& lf, double tol) const; | ||||
| 	bool empty() const; | ||||
| 	Vector getb() const; | ||||
| 	double error(const VectorValues& c) const; | ||||
| }; | ||||
| 
 | ||||
| class GaussianFactorSet { | ||||
|   GaussianFactorSet(); | ||||
|   void push_back(GaussianFactor* factor); | ||||
|  |  | |||
|  | @ -120,7 +120,7 @@ void Gaussian::WhitenInPlace(MatrixColMajor& H) const { | |||
| } | ||||
| 
 | ||||
| // General QR, see also special version in Constrained
 | ||||
| SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<long>&> firstZeroRows) const { | ||||
| /*SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<long>&> firstZeroRows) const {
 | ||||
| 
 | ||||
| 	// get size(A) and maxRank
 | ||||
| 	// TODO: really no rank problems ?
 | ||||
|  | @ -137,12 +137,87 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<long>&> firstZero | |||
| 	else | ||||
| 	  householder_denseqr(Ab); | ||||
| #else | ||||
| 	householder(Ab, maxRank); | ||||
| householder(Ab, maxRank); | ||||
| #endif | ||||
| 
 | ||||
| return Unit::Create(maxRank); | ||||
| }*/ | ||||
| 
 | ||||
| // Special version of QR for Constrained calls slower but smarter code
 | ||||
| // that deals with possibly zero sigmas
 | ||||
| // It is Gram-Schmidt orthogonalization rather than Householder
 | ||||
| // Previously Diagonal::QR
 | ||||
| SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firstZeroRows) const { | ||||
| 
 | ||||
| 	WhitenInPlace(Ab); | ||||
| 	// get size(A) and maxRank
 | ||||
| 	size_t m = Ab.size1(), n = Ab.size2()-1; | ||||
| 	size_t maxRank = min(m,n); | ||||
| 
 | ||||
| 	// create storage for [R d]
 | ||||
| 	typedef boost::tuple<size_t, Vector, double> Triple; | ||||
| 	list<Triple> Rd; | ||||
| 
 | ||||
| 	Vector pseudo(m); // allocate storage for pseudo-inverse
 | ||||
| 	Vector weights = ones(m); // calculate weights once
 | ||||
| 
 | ||||
| 	// We loop over all columns, because the columns that can be eliminated
 | ||||
| 	// are not necessarily contiguous. For each one, estimate the corresponding
 | ||||
| 	// scalar variable x as d-rS, with S the separator (remaining columns).
 | ||||
| 	// Then update A and b by substituting x with d-rS, zero-ing out x's column.
 | ||||
| 	for (size_t j=0; j<n; ++j) { | ||||
| 		// extract the first column of A
 | ||||
| 		// ublas::matrix_column is slower ! TODO Really, why ????
 | ||||
| 		//  AGC: if you use column() you will automatically call ublas, use
 | ||||
| 		//      column_() to actually use the one in our library
 | ||||
| 		Vector a(column(Ab, j)); | ||||
| 
 | ||||
| 		// Calculate weighted pseudo-inverse and corresponding precision
 | ||||
| 		double precision = weightedPseudoinverse(a, weights, pseudo); | ||||
| 
 | ||||
| 		// If precision is zero, no information on this column
 | ||||
| 		// This is actually not limited to constraints, could happen in Gaussian::QR
 | ||||
| 		// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
 | ||||
| 		if (precision < 1e-8) continue; | ||||
| 
 | ||||
| 		// create solution [r d], rhs is automatically r(n)
 | ||||
| 		Vector rd(n+1); // uninitialized !
 | ||||
| 		rd(j)=1.0; // put 1 on diagonal
 | ||||
| 		for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
 | ||||
| 			rd(j2) = inner_prod(pseudo, ublas::matrix_column<Matrix>(Ab, j2)); | ||||
| 
 | ||||
| 		// construct solution (r, d, sigma)
 | ||||
| 		Rd.push_back(boost::make_tuple(j, rd, precision)); | ||||
| 
 | ||||
| 		// exit after rank exhausted
 | ||||
| 		if (Rd.size()>=maxRank) break; | ||||
| 
 | ||||
| 		// update Ab, expensive, using outer product
 | ||||
| 		updateAb(Ab, j, a, rd); | ||||
| 	} | ||||
| 
 | ||||
| 	// Create storage for precisions
 | ||||
| 	Vector precisions(Rd.size()); | ||||
| 
 | ||||
| 	// Write back result in Ab, imperative as we are
 | ||||
| 	// TODO: test that is correct if a column was skipped !!!!
 | ||||
| 	size_t i = 0; // start with first row
 | ||||
| 	bool mixed = false; | ||||
| 	BOOST_FOREACH(const Triple& t, Rd) { | ||||
| 		const size_t& j  = t.get<0>(); | ||||
| 		const Vector& rd = t.get<1>(); | ||||
| 		precisions(i)    = t.get<2>(); | ||||
| 		if (precisions(i)==inf) mixed = true; | ||||
| 		for (size_t j2=0; j2<j; ++j2) Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
 | ||||
| 		for (size_t j2=j; j2<n+1; ++j2) // copy the j-the row TODO memcpy
 | ||||
| 			Ab(i,j2) = rd(j2); | ||||
| 		i+=1; | ||||
| 	} | ||||
| 
 | ||||
| 	return Unit::Create(maxRank); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| // General QR, see also special version in Constrained
 | ||||
| SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const { | ||||
| 
 | ||||
|  |  | |||
|  | @ -3,7 +3,7 @@ function c = createZeroDelta() | |||
| v_x1 = [0; 0]; | ||||
| v_x2 = [0; 0]; | ||||
| v_l1 = [0; 0]; | ||||
| c = VectorConfig(); | ||||
| c = VectorValues(); | ||||
| c.insert('x1', v_x1); | ||||
| c.insert('x2', v_x2); | ||||
| c.insert('l1', v_l1); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue