From 47d40b07e77f6a960697112fe9badface5baa7f3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 30 Aug 2012 14:47:01 +0000 Subject: [PATCH 02/11] Adding README file for matlab wrapper --- matlab/README-gtsam-toolbox.txt | 74 +++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 matlab/README-gtsam-toolbox.txt diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt new file mode 100644 index 000000000..c9b22f3b7 --- /dev/null +++ b/matlab/README-gtsam-toolbox.txt @@ -0,0 +1,74 @@ +================================================================================ +GTSAM - Georgia Tech Smoothing and Mapping Library + +MATLAB wrapper + +http://borg.cc.gatech.edu/projects/gtsam +================================================================================ + +This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ +library. + + +---------------------------------------- +Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) +---------------------------------------- + +If you have a newer Ubuntu system, you must make a small modification to your +MATLAB installation, due to MATLAB being distributed with an old version of +the C++ standard library. Delete or rename all files starting with +'libstdc++' in your MATLAB installation directory, typically +/usr/local/MATLAB/[version]/sys/os/[system]/ + + +---------------------------------------- +Adding the toolbox to your MATLAB path +---------------------------------------- + +To get started, first add the 'toolbox' folder to your MATLAB path - in the +MATLAB file browser, right-click on the folder and click 'Add to path -> This +folder' (do not add the subfolders to your path). + + +---------------------------------------- +Trying out the examples +---------------------------------------- + +The examples are located in the 'gtsam_examples' subfolder. You may either +run them individually at the MATLAB command line, or open the GTSAM example +GUI by running 'gtsamExamples'. Example: + +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_examples % Change to the examples directory +>> gtsamExamples % Run the GTSAM examples GUI + + +---------------------------------------- +Running the unit tests +---------------------------------------- + +The GTSAM MATLAB toolbox also has a small set of unit tests located in the +gtsam_tests directory. Example: + +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_tests % Change to the examples directory +>> test_gtsam % Run the unit tests +Starting: testJacobianFactor +Starting: testKalmanFilter +Starting: testLocalizationExample +Starting: testOdometryExample +Starting: testPlanarSLAMExample +Starting: testPose2SLAMExample +Starting: testPose3SLAMExample +Starting: testSFMExample +Starting: testStereoVOExample +Starting: testVisualISAMExample +Tests complete! + + +---------------------------------------- +Writing your own code +---------------------------------------- + +Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you +understand a few basic concepts! Please see the manual to get started. From 4f4ced041ec5181d6a9c63f51f8eb3a25fce655a Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 30 Aug 2012 18:08:56 +0000 Subject: [PATCH 03/11] Added second path to remove matlab libraries from to readme --- matlab/README-gtsam-toolbox.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt index c9b22f3b7..6f74f9806 100644 --- a/matlab/README-gtsam-toolbox.txt +++ b/matlab/README-gtsam-toolbox.txt @@ -17,8 +17,10 @@ Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) If you have a newer Ubuntu system, you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with -'libstdc++' in your MATLAB installation directory, typically -/usr/local/MATLAB/[version]/sys/os/[system]/ +'libstdc++' in your MATLAB installation directory, in paths: + + /usr/local/MATLAB/[version]/sys/os/[system]/ + /usr/local/MATLAB/[version]/bin/[system]/ ---------------------------------------- From 84b13f3270f8f5acb30b1525fabcfb1c346891b5 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 30 Aug 2012 20:12:52 +0000 Subject: [PATCH 04/11] (Ported change from trunk to 2.1.0 branch) Added eliminateFrontals function to FactorGraph, SymbolicFactorGraph, and GaussianFactorGraph - eliminates requested number of frontal variables and returns the resulting conditional and remaining factor graph --- gtsam.h | 5 +++ gtsam/inference/FactorGraph-inl.h | 52 +++++++++++++++++++++++ gtsam/inference/FactorGraph.h | 7 +++ gtsam/inference/SymbolicFactorGraph.cpp | 7 +++ gtsam/inference/SymbolicFactorGraph.h | 16 +++++-- gtsam/inference/tests/testFactorGraph.cpp | 30 ++++++++++++- gtsam/linear/GaussianFactorGraph.cpp | 7 +++ gtsam/linear/GaussianFactorGraph.h | 10 +++++ 8 files changed, 130 insertions(+), 4 deletions(-) diff --git a/gtsam.h b/gtsam.h index aebbea8b8..4aa1480df 100644 --- a/gtsam.h +++ b/gtsam.h @@ -787,6 +787,8 @@ class SymbolicFactorGraph { // Standard interface // FIXME: Must wrap FastSet for this to work //FastSet keys() const; + + pair eliminateFrontals(size_t nFrontals) const; }; #include @@ -996,6 +998,9 @@ class GaussianFactorGraph { size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; + // Inference + pair eliminateFrontals(size_t nFrontals) const; + // Building the graph void push_back(gtsam::GaussianFactor* factor); void add(Vector b); diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 66c44db52..6bd9c340c 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -23,6 +23,7 @@ #pragma once #include +#include #include #include @@ -85,6 +86,57 @@ namespace gtsam { return size_; } + /* ************************************************************************* */ + template + std::pair::sharedConditional, FactorGraph > + FactorGraph::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const + { + // Build variable index + VariableIndex variableIndex(*this); + + // Find first variable + Index firstIndex = 0; + while(firstIndex < variableIndex.size() && variableIndex[firstIndex].empty()) + ++ firstIndex; + + // Check that number of variables is in bounds + if(firstIndex + nFrontals >= variableIndex.size()) + throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph."); + + // Get set of involved factors + FastSet involvedFactorIs; + for(Index j = firstIndex; j < firstIndex + nFrontals; ++j) { + BOOST_FOREACH(size_t i, variableIndex[j]) { + involvedFactorIs.insert(i); + } + } + + // Separate factors into involved and remaining + FactorGraph involvedFactors; + FactorGraph remainingFactors; + FastSet::const_iterator involvedFactorIsIt = involvedFactorIs.begin(); + for(size_t i = 0; i < this->size(); ++i) { + if(*involvedFactorIsIt == i) { + // If the current factor is involved, add it to involved and increment involved iterator + involvedFactors.push_back((*this)[i]); + ++ involvedFactorIsIt; + } else { + // If not involved, add to remaining + remainingFactors.push_back((*this)[i]); + } + } + + // Do dense elimination on the involved factors + typename FactorGraph::EliminationResult eliminationResult = + eliminate(involvedFactors, nFrontals); + + // Add the remaining factor back into the factor graph + remainingFactors.push_back(eliminationResult.second); + + // Return the eliminated factor and remaining factor graph + return std::make_pair(eliminationResult.first, remainingFactors); + } + /* ************************************************************************* */ template void FactorGraph::replace(size_t index, sharedFactor factor) { diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 5295a4c58..ed3e7d952 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -175,6 +175,13 @@ template class BayesTree; /** Get the last factor */ sharedFactor back() const { return factors_.back(); } + /** Eliminate the first \c n frontal variables, returning the resulting + * conditional and remaining factor graph - this is very inefficient for + * eliminating all variables, to do that use EliminationTree or + * JunctionTree. + */ + std::pair > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const; + /// @} /// @name Modifying Factor Graphs (imperative, discouraged) /// @{ diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index 00f3439a0..ff7a91ca6 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -63,6 +63,13 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + std::pair + SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const + { + return FactorGraph::eliminateFrontals(nFrontals, EliminateSymbolic); + } + /* ************************************************************************* */ IndexFactor::shared_ptr CombineSymbolic( const FactorGraph& factors, const FastMap SymbolicFactorGraph(const FactorGraph& fg); + + /** Eliminate the first \c n frontal variables, returning the resulting + * conditional and remaining factor graph - this is very inefficient for + * eliminating all variables, to do that use EliminationTree or + * JunctionTree. Note that this version simply calls + * FactorGraph::eliminateFrontals with EliminateSymbolic + * as the eliminate function argument. + */ + std::pair eliminateFrontals(size_t nFrontals) const; /// @} /// @name Standard Interface @@ -68,6 +77,8 @@ namespace gtsam { */ FastSet keys() const; + + /// @} /// @name Advanced Interface /// @{ @@ -87,9 +98,8 @@ namespace gtsam { }; /** Create a combined joint factor (new style for EliminationTree). */ - IndexFactor::shared_ptr CombineSymbolic( - const FactorGraph& factors, const FastMap >& variableSlots); + IndexFactor::shared_ptr CombineSymbolic(const FactorGraph& factors, + const FastMap >& variableSlots); /** * CombineAndEliminate provides symbolic elimination. diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 4743d1102..6121a7f99 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -21,16 +21,44 @@ #include #include #include // for operator += +#include using namespace boost::assign; #include +#include #include using namespace std; using namespace gtsam; -typedef boost::shared_ptr shared; +/* ************************************************************************* */ +TEST(FactorGraph, eliminateFrontals) { + + SymbolicFactorGraph sfgOrig; + sfgOrig.push_factor(0,1); + sfgOrig.push_factor(0,2); + sfgOrig.push_factor(1,3); + sfgOrig.push_factor(1,4); + sfgOrig.push_factor(2,3); + sfgOrig.push_factor(4,5); + + IndexConditional::shared_ptr actualCond; + SymbolicFactorGraph actualSfg; + boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2, EliminateSymbolic); + + vector condIndices; + condIndices += 0,1,2,3,4; + IndexConditional expectedCond(condIndices, 2); + + SymbolicFactorGraph expectedSfg; + expectedSfg.push_factor(2,3); + expectedSfg.push_factor(4,5); + expectedSfg.push_factor(2,3,4); + + EXPECT(assert_equal(expectedSfg, actualSfg)); + EXPECT(assert_equal(expectedCond, *actualCond)); +} ///* ************************************************************************* */ // SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index e2598b1ac..02f37915c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -47,6 +47,13 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + std::pair + GaussianFactorGraph::eliminateFrontals(size_t nFrontals) const + { + return FactorGraph::eliminateFrontals(nFrontals, EliminateQR); + } + /* ************************************************************************* */ void GaussianFactorGraph::permuteWithInverse( const Permutation& inversePermutation) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index fd1ec1f6c..51748b79b 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -132,6 +132,16 @@ namespace gtsam { typedef FastSet Keys; Keys keys() const; + + /** Eliminate the first \c n frontal variables, returning the resulting + * conditional and remaining factor graph - this is very inefficient for + * eliminating all variables, to do that use EliminationTree or + * JunctionTree. Note that this version simply calls + * FactorGraph::eliminateFrontals with EliminateQR as the + * eliminate function argument. + */ + std::pair eliminateFrontals(size_t nFrontals) const; + /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); From 421145df1d653193c7f246f438cf7e9dbec8641d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 30 Aug 2012 20:13:00 +0000 Subject: [PATCH 05/11] Fixed error in unit test --- gtsam/inference/tests/testFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 6121a7f99..b2f9bc3f1 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -45,7 +45,7 @@ TEST(FactorGraph, eliminateFrontals) { IndexConditional::shared_ptr actualCond; SymbolicFactorGraph actualSfg; - boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2, EliminateSymbolic); + boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); vector condIndices; condIndices += 0,1,2,3,4; From a16f91d403726be3656aef3f07150c835da344fc Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 30 Aug 2012 20:17:51 +0000 Subject: [PATCH 06/11] Install MATLAB toolbox README along with toolbox --- matlab/CMakeLists.txt | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index b4e43d3d6..0be3863a1 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -1,13 +1,5 @@ # Install matlab components -# Utility functions -message(STATUS "Installing Matlab Utility Functions") -# Matlab files: *.m and *.fig -file(GLOB matlab_utils_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.m") -file(GLOB matlab_utils_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.fig") -set(matlab_utils ${matlab_utils_m} ${matlab_utils_fig}) -install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/) - # Tests message(STATUS "Installing Matlab Toolbox Tests") install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") @@ -21,6 +13,7 @@ install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION " # Utilities message(STATUS "Installing Matlab Toolbox Utilities") install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}") message(STATUS "Installing Matlab Toolbox Examples (Data)") # Data files: *.graph and *.txt From 5706481c06edba00accc8c99de339720d6677cc2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 31 Aug 2012 15:15:16 +0000 Subject: [PATCH 07/11] Fixed iterator checking bug in eliminateFrontals --- gtsam/inference/FactorGraph-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 6bd9c340c..97a7fa1c6 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -116,7 +116,7 @@ namespace gtsam { FactorGraph remainingFactors; FastSet::const_iterator involvedFactorIsIt = involvedFactorIs.begin(); for(size_t i = 0; i < this->size(); ++i) { - if(*involvedFactorIsIt == i) { + if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) { // If the current factor is involved, add it to involved and increment involved iterator involvedFactors.push_back((*this)[i]); ++ involvedFactorIsIt; From 91559d3c255f4ea5751afa83a0a243dfbb05de79 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 31 Aug 2012 15:19:22 +0000 Subject: [PATCH 08/11] Added info on MEX_COMMAND for building toolbox from source --- README | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README b/README index 729c3fdf1..1a992292a 100644 --- a/README +++ b/README @@ -148,6 +148,12 @@ just the geometry tests, run "make check.geometry". Individual tests can be run appending ".run" to the name of the test, for example, to run testMatrix, run "make testMatrix.run". +MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your +shell's PATH environment variable. mex is installed with matlab at +$MATLABROOT/bin/mex + +$MATLABROOT can be found by executing the command 'matlabroot' in MATLAB + Debugging tips: Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks From 0dc05f5d4e8ab793f2a624f1f622fc66ed4e3ab8 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Sep 2012 15:02:08 +0000 Subject: [PATCH 09/11] Updated matlab package scripts --- package_scripts/compile_static_boost.sh | 8 ++++++++ package_scripts/toolbox_package_unix.sh | 2 ++ 2 files changed, 10 insertions(+) create mode 100644 package_scripts/compile_static_boost.sh diff --git a/package_scripts/compile_static_boost.sh b/package_scripts/compile_static_boost.sh new file mode 100644 index 000000000..466283044 --- /dev/null +++ b/package_scripts/compile_static_boost.sh @@ -0,0 +1,8 @@ +#!/bin/sh + +# Compile boost statically, with -fPIC to allow linking it into the mex +# module (which is a dynamic library). --disable-icu prevents depending +# on libicu, which is unneeded and would require then linking the mex +# module with it as well. We just stage instead of install, then the +# toolbox_package_unix.sh script uses the staged boost. +./b2 link=static threading=multi cxxflags=-fPIC cflags=-fPIC --disable-icu -a stage \ No newline at end of file diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh index b78649e15..6f5cda526 100644 --- a/package_scripts/toolbox_package_unix.sh +++ b/package_scripts/toolbox_package_unix.sh @@ -7,6 +7,8 @@ if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then platform=lin64 elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then platform=lin32 +elif [ "$os" = "Darwin" -a "$arch" = "x86_64" ]; then + platform=mac64 else echo "Unrecognized platform" exit 1 From 453c5132e13e9b67b70688633ad74da5e34b5f5a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Sep 2012 18:01:54 +0000 Subject: [PATCH 10/11] Adjusted test tolerance to pass on 32-bit systems --- gtsam/base/tests/testCholesky.cpp | 42 +++++++++++++++---------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 55c72543e..8e8da7a70 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -81,38 +81,38 @@ TEST(cholesky, BadScalingSVD) { double expectedCondition = 1e40; double actualCondition = S(0) / S(1); - DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41); + DOUBLES_EQUAL(expectedCondition, actualCondition, 1e30); } /* ************************************************************************* */ TEST(cholesky, underconstrained) { Matrix L(6,6); L << - 1, 0, 0, 0, 0, 0, - 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0, - 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0, - 1.12108665967793, 1.01584408366945, 1, 0, 0, 0, - 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1, + 1, 0, 0, 0, 0, 0, + 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0, + 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0, + 1.12108665967793, 1.01584408366945, 1, 0, 0, 0, + 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1, 0.1197220685104, 1, 0, 0, 0, 0; Matrix D1(6,6); D1 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, 1.34342584865901, 0, + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 1.34342584865901, 0, 0, 0, 0, 0, 0, 1e-12; Matrix D2(6,6); D2 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, 0, 0, + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; Matrix D3(6,6); D3 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, -0.5, 0, + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, -0.5, 0, 0, 0, 0, 0, 0, -0.6; Matrix A1 = L * D1 * L.transpose(); From 9816d4fe611664969d6eae58c950abf3c7f2c55a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Sep 2012 18:24:39 +0000 Subject: [PATCH 11/11] Fixed key bug in GeneralSFMFactor_Cal3Bundler --- gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 1b1ae5390..21f5f2008 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -366,7 +366,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { } // Constrain position of system with the first camera constrained to the origin - graph.addCameraConstraint(X(0), cameras[0]); + graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));