12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in fix/zeroRowInQR (pull request #155) 

Change in QR factorization
release/4.3a0
Frank Dellaert 2015-06-17 13:59:28 -07:00
parent 19bc1cd686 82f6ca64a5
commit e7d10b8080
4 changed files with 100 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/linear/HessianFactor.cpp
View File

@ -268,18 +268,11 @@ void HessianFactor::print(const std::string& s,
/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const HessianFactor*>(&lf))
const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
if (!rhs || !Factor::equals(lf, tol))
return false;
else {
if (!Factor::equals(lf, tol))
return false;
Matrix thisMatrix = info_.full().selfadjointView();
thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
Matrix rhsMatrix =
static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
}
return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(),
tol);
}
/* ************************************************************************* */

20
gtsam/linear/JacobianFactor.cpp
View File

@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() :
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
// Copy the matrix data depending on what type of factor we're copying from
if (const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
*this = JacobianFactor(*rhs);
else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
*this = JacobianFactor(*rhs);
if (const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(&gf))
*this = JacobianFactor(*asJacobian);
else if (const HessianFactor* asHessian = dynamic_cast<const HessianFactor*>(&gf))
*this = JacobianFactor(*asHessian);
else
throw std::invalid_argument(
"In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
@ -432,8 +432,6 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
/* ************************************************************************* */
double JacobianFactor::error(const VectorValues& c) const {
if (empty())
return 0;
Vector weighted = error_vector(c);
return 0.5 * weighted.dot(weighted);
}
@ -729,8 +727,8 @@ std::pair<boost::shared_ptr<GaussianConditional>,
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
// Split elimination result into conditional and remaining factor
GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(
keys.size());
GaussianConditional::shared_ptr conditional = //
jointFactor->splitConditional(keys.size());
return make_pair(conditional, jointFactor);
}
@ -759,11 +757,11 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
}
GaussianConditional::shared_ptr conditional = boost::make_shared<
GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd)
const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd)
- Ab_.rowStart() - frontalDim;
const DenseIndex remainingRows =
model_ ?
std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) :
model_ ? std::min(model_->sigmas().size() - frontalDim,
maxRemainingRows) :
maxRemainingRows;
Ab_.rowStart() += frontalDim;
Ab_.rowEnd() = Ab_.rowStart() + remainingRows;

113
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -280,30 +280,69 @@ TEST(HessianFactor, ConstructorNWay)
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate)
{
Matrix A01 = (Matrix(3,3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0).finished();
TEST(HessianFactor, CombineAndEliminate1) {
Matrix3 A01 = 3.0 * I_3x3;
Vector3 b0(1, 0, 0);
Matrix3 A21 = 4.0 * I_3x3;
Vector3 b2 = Vector3::Zero();
GaussianFactorGraph gfg;
gfg.add(1, A01, b0);
gfg.add(1, A21, b2);
Matrix63 A1;
A1 << A01, A21;
Vector6 b;
b << b0, b2;
// create a full, uneliminated version of the factor
JacobianFactor jacobian(1, A1, b);
// Make sure combining works
HessianFactor hessian(gfg);
VectorValues v;
v.insert(1, Vector3(1, 0, 0));
EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9);
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9));
EXPECT(
assert_equal(jacobian.augmentedInformation(),
hessian.augmentedInformation(), 1e-9));
// perform elimination on jacobian
Ordering ordering = list_of(1);
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = //
jacobian.eliminate(ordering);
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
EXPECT(
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate2) {
Matrix A01 = I_3x3;
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0).finished();
Matrix A11 = (Matrix(3,3) <<
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0).finished();
Matrix A10 = 2.0 * I_3x3;
Matrix A11 = -2.0 * I_3x3;
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0).finished();
Matrix A21 = 3.0 * I_3x3;
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
@ -312,29 +351,45 @@ TEST(HessianFactor, CombineAndEliminate)
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix93 A0; A0 << A10, Z_3x3, Z_3x3;
Matrix93 A1; A1 << A11, A01, A21;
Vector9 b; b << b1, b0, b2;
Vector9 sigmas; sigmas << s1, s0, s2;
Matrix93 A0, A1;
A0 << A10, Z_3x3, Z_3x3;
A1 << A11, A01, A21;
Vector9 b, sigmas;
b << b1, b0, b2;
sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
JacobianFactor jacobian(0, A0, 1, A1, b,
noiseModel::Diagonal::Sigmas(sigmas, true));
// Make sure combining works
EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
HessianFactor hessian(gfg);
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
EXPECT(
assert_equal(jacobian.augmentedInformation(),
hessian.augmentedInformation(), 1e-9));
// perform elimination on jacobian
Ordering ordering = list_of(0);
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedRemainingFactor;
boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0)));
JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = //
jacobian.eliminate(ordering);
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualCholeskyFactor;
boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
VectorValues v;
v.insert(1, Vector3(1, 2, 3));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
EXPECT(
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}
/* ************************************************************************* */

6
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -540,9 +540,9 @@ TEST(JacobianFactor, EliminateQR)
EXPECT(assert_equal(size_t(2), actualJF.keys().size()));
EXPECT(assert_equal(Key(9), actualJF.keys()[0]));
EXPECT(assert_equal(Key(11), actualJF.keys()[1]));
EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001));
EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001));
EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001));
EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001));
EXPECT(!actualJF.get_model());
}