IMPORTANT CHANGE/BUGFIX: QR elimination now (I think properly) leaves a row of zeros if the RHS after QR contains a non-zero. A corresponding change in the error calculation makes that Jacobian and Hessian factors now agree on error. I think this was a bug, because it affected the error, but (I think) it only pertained to "empty" JacobianFactors which have no bearing on optimization/elimination.
dellaert
parent
83171b6096
commit
f523db2d98
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -684,8 +682,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);
|
||||
}
|
||||
|
|
@ -714,11 +712,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;
|
||||
|
|
|
|||
|
|
@ -279,60 +279,43 @@ TEST(HessianFactor, ConstructorNWay)
|
|||
EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace example {
|
||||
Matrix A01 = (Matrix(3,3) <<
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0).finished();
|
||||
Vector3 b0(1,0,0);//(1.5, 1.5, 1.5);
|
||||
Vector3 s0=Vector3::Ones();//(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();
|
||||
Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5);
|
||||
Vector3 s1=Vector3::Ones();//(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();
|
||||
Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5);
|
||||
Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactor, CombineAndEliminate1) {
|
||||
using namespace example;
|
||||
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, noiseModel::Diagonal::Sigmas(s0, true));
|
||||
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
|
||||
gfg.add(1, A01, b0);
|
||||
gfg.add(1, A21, b2);
|
||||
|
||||
Matrix63 A1;
|
||||
A1 << A11, A21;
|
||||
Vector6 b, sigmas;
|
||||
A1 << A01, A21;
|
||||
Vector6 b;
|
||||
b << b0, b2;
|
||||
sigmas << s0, s2;
|
||||
|
||||
// create a full, uneliminated version of the factor
|
||||
JacobianFactor expectedFactor(1, A1, b,
|
||||
noiseModel::Diagonal::Sigmas(sigmas, true));
|
||||
JacobianFactor jacobian(1, A1, b);
|
||||
|
||||
// Make sure combining works
|
||||
EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
|
||||
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 expectedRemaining;
|
||||
boost::tie(expectedConditional, expectedRemaining) = //
|
||||
expectedFactor.eliminate(ordering);
|
||||
JacobianFactor::shared_ptr expectedFactor;
|
||||
boost::tie(expectedConditional, expectedFactor) = //
|
||||
jacobian.eliminate(ordering);
|
||||
|
||||
// Eliminate
|
||||
GaussianConditional::shared_ptr actualConditional;
|
||||
|
|
@ -341,15 +324,28 @@ TEST(HessianFactor, CombineAndEliminate1) {
|
|||
EliminateCholesky(gfg, ordering);
|
||||
|
||||
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
|
||||
VectorValues vv;
|
||||
vv.insert(1, Vector3(1, 2, 3));
|
||||
DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9);
|
||||
EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 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) {
|
||||
using namespace example;
|
||||
Matrix A01 = I_3x3;
|
||||
Vector3 b0(1.5, 1.5, 1.5);
|
||||
Vector3 s0(1.6, 1.6, 1.6);
|
||||
|
||||
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 = 3.0 * I_3x3;
|
||||
Vector3 b2(3.5, 3.5, 3.5);
|
||||
Vector3 s2(3.6, 3.6, 3.6);
|
||||
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
|
||||
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
|
||||
|
|
@ -363,18 +359,22 @@ TEST(HessianFactor, CombineAndEliminate2) {
|
|||
sigmas << s1, s0, s2;
|
||||
|
||||
// create a full, uneliminated version of the factor
|
||||
JacobianFactor expectedFactor(0, A0, 1, A1, b,
|
||||
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 expectedRemaining;
|
||||
boost::tie(expectedConditional, expectedRemaining) = //
|
||||
expectedFactor.eliminate(ordering);
|
||||
JacobianFactor::shared_ptr expectedFactor;
|
||||
boost::tie(expectedConditional, expectedFactor) = //
|
||||
jacobian.eliminate(ordering);
|
||||
|
||||
// Eliminate
|
||||
GaussianConditional::shared_ptr actualConditional;
|
||||
|
|
@ -383,10 +383,13 @@ TEST(HessianFactor, CombineAndEliminate2) {
|
|||
EliminateCholesky(gfg, ordering);
|
||||
|
||||
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
|
||||
VectorValues vv;
|
||||
vv.insert(1, Vector3(1, 2, 3));
|
||||
DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9);
|
||||
EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue