Merge pull request #2028 from borglab/fix/setZeroTwice
Fix small inefficiency in QR pathrelease/4.3a0
						commit
						cddc2884c6
					
				|  | @ -639,7 +639,7 @@ void inplace_QR(Matrix& A){ | |||
|   Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data()); | ||||
| #endif | ||||
| 
 | ||||
|   zeroBelowDiagonal(A); | ||||
|   A.triangularView<Eigen::StrictlyLower>().setZero(); | ||||
| } | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -216,19 +216,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { | |||
|   return A.row(j); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Zeros all of the elements below the diagonal of a matrix, in place | ||||
|  * @param A is a matrix, to be modified in place | ||||
|  * @param cols is the number of columns to zero, use zero for all columns | ||||
|  */ | ||||
| template<class MATRIX> | ||||
| void zeroBelowDiagonal(MATRIX& A, size_t cols=0) { | ||||
|   const size_t m = A.rows(), n = A.cols(); | ||||
|   const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n); | ||||
|   for (size_t j=0; j<k; ++j) | ||||
|     A.col(j).segment(j+1, m-(j+1)).setZero(); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * static transpose function, just calls Eigen transpose member function | ||||
|  */ | ||||
|  |  | |||
|  | @ -596,61 +596,6 @@ TEST(Matrix, scalar_divide ) | |||
|   EQUALITY(B,A/10); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Matrix, zero_below_diagonal ) { | ||||
|   Matrix A1 = (Matrix(3, 4) << | ||||
|       1.0, 2.0, 3.0, 4.0, | ||||
|       1.0, 2.0, 3.0, 4.0, | ||||
|       1.0, 2.0, 3.0, 4.0).finished(); | ||||
| 
 | ||||
|   Matrix expected1 = (Matrix(3, 4) << | ||||
|       1.0, 2.0, 3.0, 4.0, | ||||
|       0.0, 2.0, 3.0, 4.0, | ||||
|       0.0, 0.0, 3.0, 4.0).finished(); | ||||
|   Matrix actual1r = A1; | ||||
|   zeroBelowDiagonal(actual1r); | ||||
|   EXPECT(assert_equal(expected1, actual1r, 1e-10)); | ||||
| 
 | ||||
|   Matrix actual1c = A1; | ||||
|   zeroBelowDiagonal(actual1c); | ||||
|   EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); | ||||
| 
 | ||||
|   actual1c = A1; | ||||
|   zeroBelowDiagonal(actual1c, 4); | ||||
|   EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); | ||||
| 
 | ||||
|   Matrix A2 = (Matrix(5, 3) << | ||||
|         1.0, 2.0, 3.0, | ||||
|         1.0, 2.0, 3.0, | ||||
|         1.0, 2.0, 3.0, | ||||
|         1.0, 2.0, 3.0, | ||||
|         1.0, 2.0, 3.0).finished(); | ||||
|   Matrix expected2 = (Matrix(5, 3) << | ||||
|       1.0, 2.0, 3.0, | ||||
|       0.0, 2.0, 3.0, | ||||
|       0.0, 0.0, 3.0, | ||||
|       0.0, 0.0, 0.0, | ||||
|       0.0, 0.0, 0.0).finished(); | ||||
| 
 | ||||
|   Matrix actual2r = A2; | ||||
|   zeroBelowDiagonal(actual2r); | ||||
|   EXPECT(assert_equal(expected2, actual2r, 1e-10)); | ||||
| 
 | ||||
|   Matrix actual2c = A2; | ||||
|   zeroBelowDiagonal(actual2c); | ||||
|   EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); | ||||
| 
 | ||||
|   Matrix expected2_partial = (Matrix(5, 3) << | ||||
|         1.0, 2.0, 3.0, | ||||
|         0.0, 2.0, 3.0, | ||||
|         0.0, 2.0, 3.0, | ||||
|         0.0, 2.0, 3.0, | ||||
|         0.0, 2.0, 3.0).finished(); | ||||
|   actual2c = A2; | ||||
|   zeroBelowDiagonal(actual2c, 1); | ||||
|   EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Matrix, inverse ) | ||||
| { | ||||
|  | @ -825,7 +770,7 @@ TEST(Matrix, eigen_QR ) | |||
|       10, 0, 0,  0,-10,0,   2, | ||||
|       00, 10,0, 0, 0, -10, -1).finished()); | ||||
|   Matrix actual = A.householderQr().matrixQR(); | ||||
|   zeroBelowDiagonal(actual); | ||||
|   actual.triangularView<Eigen::StrictlyLower>().setZero(); | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, actual, 1e-3)); | ||||
| 
 | ||||
|  |  | |||
|  | @ -827,8 +827,6 @@ std::pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr> Eliminate | |||
|     // The inplace variant will have no valid rows anymore below m==n
 | ||||
|     // and only entries above the diagonal are valid.
 | ||||
|     inplace_QR(Ab.matrix()); | ||||
|     // We zero below the diagonal to agree with the result from noieModel QR
 | ||||
|     Ab.matrix().triangularView<Eigen::StrictlyLower>().setZero(); | ||||
|     size_t m = Ab.rows(), n = Ab.cols() - 1; | ||||
|     size_t maxRank = min(m, n); | ||||
|     jointFactor->model_ = noiseModel::Unit::Create(maxRank); | ||||
|  |  | |||
|  | @ -191,8 +191,6 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { | |||
| 
 | ||||
|   gttic(Gaussian_noise_model_QR); | ||||
| 
 | ||||
|   static const bool debug = false; | ||||
| 
 | ||||
|   // get size(A) and maxRank
 | ||||
|   // TODO: really no rank problems ?
 | ||||
|    size_t m = Ab.rows(), n = Ab.cols()-1; | ||||
|  | @ -201,15 +199,8 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { | |||
|   // pre-whiten everything (cheaply if possible)
 | ||||
|   WhitenInPlace(Ab); | ||||
| 
 | ||||
|   if(debug) gtsam::print(Ab, "Whitened Ab: "); | ||||
| 
 | ||||
|   // Eigen QR - much faster than older householder approach
 | ||||
|   inplace_QR(Ab); | ||||
|   Ab.triangularView<Eigen::StrictlyLower>().setZero(); | ||||
| 
 | ||||
|   // hand-coded householder implementation
 | ||||
|   // TODO: necessary to isolate last column?
 | ||||
|   // householder(Ab, maxRank);
 | ||||
| 
 | ||||
|   return noiseModel::Unit::Create(maxRank); | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue