diff --git a/.cproject b/.cproject index 2bc98db3c..677af8430 100644 --- a/.cproject +++ b/.cproject @@ -307,6 +307,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -333,7 +341,6 @@ make - tests/testBayesTree.run true false @@ -341,7 +348,6 @@ make - testBinaryBayesNet.run true false @@ -389,7 +395,6 @@ make - testSymbolicBayesNet.run true false @@ -397,7 +402,6 @@ make - tests/testSymbolicFactor.run true false @@ -405,7 +409,6 @@ make - testSymbolicFactorGraph.run true false @@ -421,20 +424,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -523,22 +517,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -555,6 +533,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -579,26 +573,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -683,26 +677,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -1045,7 +1039,6 @@ make - testGraph.run true false @@ -1053,7 +1046,6 @@ make - testJunctionTree.run true false @@ -1061,7 +1053,6 @@ make - testSymbolicBayesNetB.run true false @@ -1221,7 +1212,6 @@ make - testErrors.run true false @@ -1267,10 +1257,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1355,10 +1345,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1693,6 +1683,7 @@ make + testSimulated2DOriented.run true false @@ -1732,6 +1723,7 @@ make + testSimulated2D.run true false @@ -1739,6 +1731,7 @@ make + testSimulated3D.run true false @@ -1832,6 +1825,14 @@ true true + + make + -j5 + testJacobianFactor.run + true + true + true + make -j2 @@ -1930,6 +1931,7 @@ make + tests/testGaussianISAM2 true false @@ -1951,6 +1953,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -2152,7 +2250,6 @@ cpack - -G DEB true false @@ -2160,7 +2257,6 @@ cpack - -G RPM true false @@ -2168,7 +2264,6 @@ cpack - -G TGZ true false @@ -2176,7 +2271,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2310,98 +2404,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2445,38 +2475,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9b97cf6bf..dc7beaa2c 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -324,9 +324,18 @@ Matrix HessianFactor::information() const { double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) const double f = constantTerm(); - const double xtg = c.vector().dot(linearTerm()); - const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * c.vector(); - + double xtg = 0, xGx = 0; + if (c.dim() == this->rows()) { + // If using the full vector values, this will reduce copying + xtg = c.vector().dot(linearTerm()); + xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * c.vector(); + } else { + // extract the relevant subset of the VectorValues + // NOTE may not be as efficient + const Vector x = c.subvector(this->keys()); + xtg = x.dot(linearTerm()); + xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; + } return 0.5 * (f - 2.0 * xtg + xGx); } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 4dffd173c..bdec5a572 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -202,4 +202,28 @@ void VectorValues::swap(VectorValues& other) { this->maps_.swap(other.maps_); } -} \ No newline at end of file +/* ************************************************************************* */ +Vector VectorValues::subvector(const std::vector& indices) const { + if (indices.empty()) + return Vector(); + + // find dimensions + size_t d = 0; + BOOST_FOREACH(const Index& idx, indices) + d += dim(idx); + + // copy out values + Vector result(d); + size_t curHead = 0; + BOOST_FOREACH(const Index& j, indices) { + const SubVector& vj = at(j); + size_t dj = (size_t) vj.rows(); + result.segment(curHead, dj) = vj; + curHead += dj; + } + return result; +} + +/* ************************************************************************* */ + +} // \namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 9cb278b10..0d9473c69 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -256,6 +256,9 @@ namespace gtsam { /** Reference the entire solution vector. */ Vector& vector() { chk(); return values_; } + /** Access a vector that is a subset of relevant indices */ + Vector subvector(const std::vector& indices) const; + /** Check whether this VectorValues has the same structure, meaning has the * same number of variables and that all variables are of the same dimension, * as another VectorValues diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 05d2fdbd5..caaff6748 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -197,6 +197,14 @@ TEST(HessianFactor, Constructor2) EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin()))); EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1))); EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); + + // Check case when vector values is larger than factor + dims.push_back(2); + VectorValues dxLarge(dims); + dxLarge[0] = dx0; + dxLarge[1] = dx1; + dxLarge[2] = Vector_(2, 0.1, 0.2); + EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 33b1b41cc..0d0d9124d 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -132,6 +132,15 @@ TEST(JacobianFactor, error) { Vector expected_whitened = Vector_(3, 4., 5., 6.); Vector actual_whitened = jf.error_vector(values); EXPECT(assert_equal(expected_whitened, actual_whitened)); + + // check behavior when there are more values than in this factor + VectorValues largeValues(3, 3); + largeValues[0] = Vector_(3, 1.,2.,3.); + largeValues[1] = Vector_(3, 4.,5.,6.); + largeValues[2] = Vector_(3, 7.,8.,9.); + + EXPECT(assert_equal(expected_unwhitened, jf.unweighted_error(largeValues))); + EXPECT(assert_equal(expected_whitened, jf.error_vector(largeValues))); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index d6cdde4e8..c660d4a0d 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -449,7 +449,19 @@ TEST(VectorValues, permute) { } /* ************************************************************************* */ -int main() { - TestResult tr; return TestRegistry::runAllTests(tr); +TEST(VectorValues, subvector) { + VectorValues init; + init.insert(0, Vector_(1, 1.0)); + init.insert(1, Vector_(2, 2.0, 3.0)); + init.insert(2, Vector_(2, 4.0, 5.0)); + init.insert(3, Vector_(2, 6.0, 7.0)); + + std::vector indices; + indices += 0, 2, 3; + Vector expSubVector = Vector_(5, 1.0, 4.0, 5.0, 6.0, 7.0); + EXPECT(assert_equal(expSubVector, init.subvector(indices))); } + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */