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