diff --git a/.gitignore b/.gitignore index b62617d21..2682a6748 100644 --- a/.gitignore +++ b/.gitignore @@ -16,3 +16,4 @@ cython/gtsam.pyx cython/gtsam.so cython/gtsam_wrapper.pxd .vscode +.env diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 2d8b7f061..d41ba7f26 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -1,9 +1,8 @@ -# This is a sample build configuration for C++ – Make. -# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples. -# Only use spaces to indent your .yml configuration. +# Built from sample configuration for C++ – Make. +# Check https://confluence.atlassian.com/x/5Q4SMw for more examples. # ----- -# You can specify a custom docker image from Docker Hub as your build environment. -image: ilssim/cmake-boost-qt5 +# Our custom docker image from Docker Hub as the build environment. +image: dellaert/ubuntu-boost-tbb-eigen3:bionic pipelines: default: @@ -11,6 +10,6 @@ pipelines: script: # Modify the commands below to build your repository. - mkdir build - cd build - - cmake .. - - make - - make check \ No newline at end of file + - cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF .. + - make -j2 + - make -j2 check \ No newline at end of file diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile new file mode 100644 index 000000000..33aa1ab96 --- /dev/null +++ b/docker/ubuntu-boost-tbb-eigen3/Dockerfile @@ -0,0 +1,18 @@ +# Get the base Ubuntu image from Docker Hub +FROM ubuntu:bionic + +# Update apps on the base image +RUN apt-get -y update && apt-get install -y + +# Install C++ +RUN apt-get -y install build-essential + +# Install boost and cmake +RUN apt-get -y install libboost-all-dev cmake + +# Install TBB +RUN apt-get -y install libtbb-dev + +# Install latest Eigen +RUN apt-get install -y libeigen3-dev + diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp new file mode 100644 index 000000000..8331ade04 --- /dev/null +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -0,0 +1,120 @@ +/** + * @file ISAM2Example_SmartFactor.cpp + * @brief test of iSAM with smart factors, led to bitbucket issue #367 + * @author Alexander (pumaking on BitBucket) + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::P; +using symbol_shorthand::X; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +int main(int argc, char* argv[]) { + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + Vector6 sigmas; + sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1); + auto noise = noiseModel::Diagonal::Sigmas(sigmas); + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + parameters.cacheLinearizedFactors = false; + parameters.enableDetailedResults = true; + parameters.print(); + ISAM2 isam(parameters); + + // Create a factor graph + NonlinearFactorGraph graph; + Values initialEstimate; + + Point3 point(0.0, 0.0, 1.0); + + // Intentionally initialize the variables off from the ground truth + Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20)); + + Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0)); + Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0)); + Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0)); + Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0)); + Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0)); + + vector poses = {pose1, pose2, pose3, pose4, pose5}; + + // Add first pose + graph.emplace_shared>(X(0), poses[0], noise); + initialEstimate.insert(X(0), poses[0].compose(delta)); + + // Create smart factor with measurement from first pose only + SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); + smartFactor->add(PinholePose(poses[0], K).project(point), X(0)); + graph.push_back(smartFactor); + + // loop over remaining poses + for (size_t i = 1; i < 5; i++) { + cout << "****************************************************" << endl; + cout << "i = " << i << endl; + + // Add prior on new pose + graph.emplace_shared>(X(i), poses[i], noise); + initialEstimate.insert(X(i), poses[i].compose(delta)); + + // "Simulate" measurement from this pose + PinholePose camera(poses[i], K); + Point2 measurement = camera.project(point); + cout << "Measurement " << i << "" << measurement << endl; + + // Add measurement to smart factor + smartFactor->add(measurement, X(i)); + + // Update iSAM2 + ISAM2Result result = isam.update(graph, initialEstimate); + result.print(); + + cout << "Detailed results:" << endl; + for (auto keyedStatus : result.detail->variableStatus) { + const auto& status = keyedStatus.second; + PrintKey(keyedStatus.first); + cout << " {" << endl; + cout << "reeliminated: " << status.isReeliminated << endl; + cout << "relinearized above thresh: " << status.isAboveRelinThreshold + << endl; + cout << "relinearized involved: " << status.isRelinearizeInvolved << endl; + cout << "relinearized: " << status.isRelinearized << endl; + cout << "observed: " << status.isObserved << endl; + cout << "new: " << status.isNew << endl; + cout << "in the root clique: " << status.inRootClique << endl; + cout << "}" << endl; + } + + Values currentEstimate = isam.calculateEstimate(); + currentEstimate.print("Current estimate: "); + + boost::optional pointEstimate = smartFactor->point(currentEstimate); + if (pointEstimate) { + cout << *pointEstimate << endl; + } else { + cout << "Point degenerate." << endl; + } + + // Reset graph and initial estimate for next iteration + graph.resize(0); + initialEstimate.clear(); + } + + return 0; +} diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index deaf3b781..63c512edb 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -206,7 +206,7 @@ int main(int argc, char *argv[]) { } #ifdef GTSAM_USE_TBB - std::auto_ptr init; + std::unique_ptr init; if(nThreads > 0) { cout << "Using " << nThreads << " threads" << endl; init.reset(new tbb::task_scheduler_init(nThreads)); diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 157768be7..751b776f6 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -11,8 +11,8 @@ /** * @file VisualISAM2Example.cpp - * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset - * This version uses iSAM2 to solve the problem incrementally + * @brief A visualSLAM example for the structure-from-motion problem on a + * simulated dataset This version uses iSAM2 to solve the problem incrementally * @author Duy-Nguyen Ta */ @@ -25,27 +25,28 @@ // For loading the data #include "SFMdata.h" -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// Camera observations of landmarks will be stored as Point2 (x, y). #include -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols +// Each variable in the system (poses and landmarks) must be identified with a +// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols +// (X1, X2, L1). Here we will use Symbols #include -// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so -// include iSAM2 here +// We want to use iSAM2 to solve the structure-from-motion problem +// incrementally, so include iSAM2 here #include -// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, -// and initial guesses for any new variables used in the added factors +// iSAM2 requires as input a set of new factors to be added stored in a factor +// graph, and initial guesses for any new variables used in the added factors #include #include -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Projection factors to model the camera's landmark observations. -// Also, we will initialize the robot at some location using a Prior factor. +// In GTSAM, measurement functions are represented as 'factors'. Several common +// factors have been provided with the library for solving robotics/SLAM/Bundle +// Adjustment problems. Here we will use Projection factors to model the +// camera's landmark observations. Also, we will initialize the robot at some +// location using a Prior factor. #include #include @@ -56,12 +57,11 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Define the camera calibration parameters Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + // Define the camera observation noise model, 1 pixel stddev + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // Create the set of ground-truth landmarks vector points = createPoints(); @@ -69,10 +69,12 @@ int main(int argc, char* argv[]) { // Create the set of ground-truth poses vector poses = createPoses(); - // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization - // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter - // structure is available that allows the user to set various properties, such as the relinearization threshold - // and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps + // to maintain proper linearization and efficient variable ordering, iSAM2 + // performs partial relinearization/reordering at each step. A parameter + // structure is available that allows the user to set various properties, such + // as the relinearization threshold and type of linear solver. For this + // example, we we set the relinearization threshold small so the iSAM2 result // will approach the batch result. ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; @@ -83,44 +85,52 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; Values initialEstimate; - // Loop over the different poses, adding the observations to iSAM incrementally + // Loop over the poses, adding the observations to iSAM incrementally for (size_t i = 0; i < poses.size(); ++i) { - // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + graph.emplace_shared >( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } // Add an initial guess for the current pose // Intentionally initialize the variables off from the ground truth - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); - // If this is the first iteration, add a prior on the first pose to set the coordinate frame - // and a prior on the first landmark to set the scale - // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before - // adding it to iSAM. - if( i == 0) { - // Add a prior on pose x0 - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); + // If this is the first iteration, add a prior on the first pose to set the + // coordinate frame and a prior on the first landmark to set the scale Also, + // as iSAM solves incrementally, we must wait until each is observed at + // least twice before adding it to iSAM. + if (i == 0) { + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)) + .finished()); + graph.emplace_shared >(Symbol('x', 0), poses[0], + kPosePrior); // Add a prior on landmark l0 - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared >(Symbol('l', 0), points[0], + kPointPrior); // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); } else { // Update iSAM with the new factors isam.update(graph, initialEstimate); - // Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. - // If accuracy is desired at the expense of time, update(*) can be called additional times - // to perform multiple optimizer iterations every step. + // Each call to iSAM2 update(*) performs one iteration of the iterative + // nonlinear solver. If accuracy is desired at the expense of time, + // update(*) can be called additional times to perform multiple optimizer + // iterations every step. isam.update(); Values currentEstimate = isam.calculateEstimate(); cout << "****************************************************" << endl; diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 9a08e3ea8..d5e8da2f0 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -1560,8 +1560,9 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ Int *dead_cols ; Int set1 ; Int set2 ; +#ifndef NDEBUG Int cs ; - +#endif int ok ; #ifndef NDEBUG @@ -1909,8 +1910,10 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ p [k] = col ; ASSERT (A [col] == EMPTY) ; - cs = CMEMBER (col) ; +#ifndef NDEBUG + cs = CMEMBER (col) ; ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; +#endif A [col] = k ; k++ ; @@ -1926,11 +1929,11 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ if (A [col] == EMPTY) { k = Col [col].shared2.order ; - cs = CMEMBER (col) ; #ifndef NDEBUG + cs = CMEMBER (col) ; dead_cols [cs]-- ; -#endif ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; +#endif DEBUG1 (("ccolamd output ordering: k "ID" col "ID " (dense or null col)\n", k, col)) ; p [k] = col ; diff --git a/gtsam/3rdparty/metis/GKlib/csr.c b/gtsam/3rdparty/metis/GKlib/csr.c index a19d793bd..4f6213c49 100644 --- a/gtsam/3rdparty/metis/GKlib/csr.c +++ b/gtsam/3rdparty/metis/GKlib/csr.c @@ -1323,55 +1323,52 @@ void gk_csr_Normalize(gk_csr_t *mat, int what, int norm) ssize_t *ptr; float *val, sum; - if (what&GK_CSR_ROW && mat->rowval) { - n = mat->nrows; + if (what & GK_CSR_ROW && mat->rowval) { + n = mat->nrows; ptr = mat->rowptr; val = mat->rowval; - #pragma omp parallel if (ptr[n] > OMPMINOPS) +#pragma omp parallel if (ptr[n] > OMPMINOPS) { - #pragma omp for private(j,sum) schedule(static) - for (i=0; i 0 */ +#pragma omp for private(j, sum) schedule(static) + for (i = 0; i < n; i++) { + for (sum = 0.0, j = ptr[i]; j < ptr[i + 1]; j++) { + if (norm == 2) + sum += val[j] * val[j]; + else if (norm == 1) + sum += val[j]; /* assume val[j] > 0 */ } if (sum > 0) { - if (norm == 2) - sum=1.0/sqrt(sum); - else if (norm == 1) - sum=1.0/sum; - for (j=ptr[i]; jcolval) { - n = mat->ncols; + if (what & GK_CSR_COL && mat->colval) { + n = mat->ncols; ptr = mat->colptr; val = mat->colval; - #pragma omp parallel if (ptr[n] > OMPMINOPS) +#pragma omp parallel if (ptr[n] > OMPMINOPS) { - #pragma omp for private(j,sum) schedule(static) - for (i=0; i 0) { - if (norm == 2) - sum=1.0/sqrt(sum); - else if (norm == 1) - sum=1.0/sum; - for (j=ptr[i]; jncon == 1) { /* return right away if the balance is OK */ - if (iabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs) + if (fabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs) return; if (graph->nbnd > 0) diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index bc5279c7b..31e4b8244 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -111,8 +111,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { return true; assert(ABC.cols() == ABC.rows()); - const Eigen::DenseIndex n = ABC.rows() - topleft; - assert(n >= 0 && nFrontal <= size_t(n)); + assert(ABC.rows() >= topleft); + const size_t n = static_cast(ABC.rows() - topleft); + assert(nFrontal <= size_t(n)); // Create views on blocks auto A = ABC.block(topleft, topleft, nFrontal, nFrontal); diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index dacb5c3fd..e0e0ecb56 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -21,7 +21,7 @@ #include #include -#include // for GTSAM_USE_TBB +#include // for GTSAM_USE_TBB #ifdef __clang__ # pragma clang diagnostic push @@ -36,13 +36,14 @@ #include #include #include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { +Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; Unit3 direction; @@ -54,7 +55,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { /* ************************************************************************* */ Unit3 Unit3::Random(boost::mt19937 & rng) { - // TODO allow any engine without including all of boost :-( + // TODO(dellaert): allow any engine without including all of boost :-( boost::uniform_on_sphere randomDirection(3); // This variate_generator object is required for versions of boost somewhere // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). @@ -161,7 +162,8 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { +double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p, + OptionalJacobian<1, 2> H_q) const { // Get the unit vectors of each, and the derivative. Matrix32 H_pn_p; Point3 pn = point3(H_p ? &H_pn_p : nullptr); @@ -185,7 +187,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, } /* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 const Vector2 xi = basis().transpose() * q.p_; if (H_q) { @@ -195,7 +197,8 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { } /* ************************************************************************* */ -Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { +Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, + OptionalJacobian<2, 2> H_q) const { // Get the point3 of this, and the derivative. Matrix32 H_qn_q; const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr); @@ -230,7 +233,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ } /* ************************************************************************* */ -double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { +double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { Matrix2 H_xi_q; const Vector2 xi = error(q, H ? &H_xi_q : nullptr); const double theta = xi.norm(); @@ -254,7 +257,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); @@ -277,4 +280,4 @@ Vector2 Unit3::localCoordinates(const Unit3& other) const { } /* ************************************************************************* */ -} +} // namespace gtsam diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index e2a5bcdea..8b9e8a7e6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -100,12 +100,12 @@ TEST( Point2, expmap) { /* ************************************************************************* */ TEST( Point2, arithmetic) { - EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); - EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); - EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); - EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); - EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); - EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); + EXPECT(assert_equal(Point2(-5, -6), -Point2(5, 6))); + EXPECT(assert_equal(Point2(5, 6), Point2(4, 5) + Point2(1, 1))); + EXPECT(assert_equal(Point2(3, 4), Point2(4, 5) - Point2(1, 1))); + EXPECT(assert_equal(Point2(8, 6), Point2(4, 3) * 2)); + EXPECT(assert_equal(Point2(4, 6), 2.0 * Point2(2, 3))); + EXPECT(assert_equal(Point2(2, 3), Point2(4, 6) / 2)); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index e04af1339..96d191419 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -83,26 +83,28 @@ namespace gtsam { std::string parent = out.str(); parent += "[label=\""; - for(Key index: clique->conditional_->frontals()) { - if(!first) parent += ","; first = false; + for (Key index : clique->conditional_->frontals()) { + if (!first) parent += ","; + first = false; parent += indexFormatter(index); } - if(clique->parent()){ + if (clique->parent()) { parent += " : "; s << parentnum << "->" << num << "\n"; } first = true; - for(Key sep: clique->conditional_->parents()) { - if(!first) parent += ","; first = false; + for (Key sep : clique->conditional_->parents()) { + if (!first) parent += ","; + first = false; parent += indexFormatter(sep); } parent += "\"];\n"; s << parent; parentnum = num; - for(sharedClique c: clique->children) { + for (sharedClique c : clique->children) { num++; saveGraph(s, c, indexFormatter, parentnum); } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index eae886785..055a03939 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -17,9 +17,13 @@ #pragma once -#include +#include +#include #include #include +#include + +#include namespace gtsam { diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index eefb6302f..60187d129 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -12,11 +12,13 @@ /** * @file GaussianConditional.cpp * @brief Conditional Gaussian Base class - * @author Christian Potthast + * @author Christian Potthast, Frank Dellaert */ -#include -#include +#include +#include +#include + #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -28,9 +30,9 @@ #pragma GCC diagnostic pop #endif -#include -#include -#include +#include +#include +#include using namespace std; @@ -54,38 +56,36 @@ namespace gtsam { BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {} /* ************************************************************************* */ - void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const - { + void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { cout << s << " Conditional density "; - for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; } cout << endl; cout << formatMatrixIndented(" R = ", get_R()) << endl; - for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { + for (const_iterator it = beginParents() ; it != endParents() ; ++it) { cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it)) << endl; } cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; - if(model_) + if (model_) model_->print(" Noise model: "); else cout << " No noise model" << endl; } /* ************************************************************************* */ - bool GaussianConditional::equals(const GaussianFactor& f, double tol) const - { - if (const GaussianConditional* c = dynamic_cast(&f)) - { + bool GaussianConditional::equals(const GaussianFactor& f, double tol) const { + if (const GaussianConditional* c = dynamic_cast(&f)) { // check if the size of the parents_ map is the same if (parents().size() != c->parents().size()) return false; // check if R_ and d_ are linear independent for (DenseIndex i = 0; i < Ab_.rows(); i++) { - list rows1; rows1.push_back(Vector(get_R().row(i))); - list rows2; rows2.push_back(Vector(c->get_R().row(i))); + list rows1, rows2; + rows1.push_back(Vector(get_R().row(i))); + rows2.push_back(Vector(c->get_R().row(i))); // check if the matrices are the same // iterate over the parents_ map @@ -109,16 +109,13 @@ namespace gtsam { return false; return true; - } - else - { + } else { return false; } } /* ************************************************************************* */ - VectorValues GaussianConditional::solve(const VectorValues& x) const - { + VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables const Vector xS = x.vector(FastVector(beginParents(), endParents())); @@ -146,8 +143,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solveOtherRHS( - const VectorValues& parents, const VectorValues& rhs) const - { + const VectorValues& parents, const VectorValues& rhs) const { // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); @@ -159,13 +155,13 @@ namespace gtsam { Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas - if(model_) + if (model_) soln.array() *= model_->sigmas().array(); // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -174,8 +170,7 @@ namespace gtsam { } /* ************************************************************************* */ - void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const - { + void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(FastVector(beginFrontals(), endFrontals())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); @@ -186,25 +181,24 @@ namespace gtsam { gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; // Scale by sigmas - if(model_) + if (model_) frontalVec.array() *= model_->sigmas().array(); // Write frontal solution into a VectorValues DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal)); vectorPosition += getDim(frontal); } } /* ************************************************************************* */ - void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const - { + void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array(); vectorPosition += getDim(frontal); } } -} +} // namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 4597759e3..56a5dc085 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -188,7 +188,7 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( m += factor->rows(); } -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) for(DenseIndex d: varDims) { assert(d != numeric_limits::max()); } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 35afddb3a..b037aad92 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -160,28 +160,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector VectorValues::vector(const FastVector& keys) const - { - // Count dimensions and collect pointers to avoid double lookups - DenseIndex totalDim = 0; - FastVector items(keys.size()); - for(size_t i = 0; i < keys.size(); ++i) { - items[i] = &at(keys[i]); - totalDim += items[i]->size(); - } - - // Copy vectors - Vector result(totalDim); - DenseIndex pos = 0; - for(const Vector *v: items) { - result.segment(pos, v->size()) = *v; - pos += v->size(); - } - - return result; - } - /* ************************************************************************* */ Vector VectorValues::vector(const Dims& keys) const { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index cb1e08f2d..f187b56de 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -26,6 +26,9 @@ #include +#include +#include + namespace gtsam { /** @@ -43,10 +46,6 @@ namespace gtsam { * - \ref exists (Key) to check if a variable is present * - Other facilities like iterators, size(), dim(), etc. * - * Indices can be non-consecutive and inserted out-of-order, but you should not - * use indices that are larger than a reasonable array size because the indices - * correspond to positions in an internal array. - * * Example: * \code VectorValues values; @@ -64,12 +63,6 @@ namespace gtsam { * *

Advanced Interface and Performance Information

* - * Internally, all vector values are stored as part of one large vector. In - * gtsam this vector is always pre-allocated for efficiency, using the - * advanced interface described below. Accessing and modifying already-allocated - * values is \f$ O(1) \f$. Using the insert() function of the standard interface - * is slow because it requires re-allocating the internal vector. - * * For advanced usage, or where speed is important: * - Allocate space ahead of time using a pre-allocating constructor * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), @@ -88,20 +81,18 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT VectorValues { - protected: + protected: typedef VectorValues This; - typedef ConcurrentMap Values; ///< Typedef for the collection of Vectors making up a VectorValues - Values values_; ///< Collection of Vectors making up this VectorValues + typedef ConcurrentMap Values; ///< Collection of Vectors making up a VectorValues + Values values_; ///< Vectors making up this VectorValues - public: - typedef Values::iterator iterator; ///< Iterator over vector values - typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - //typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair - typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair - typedef std::map Dims; + public: + typedef Values::iterator iterator; ///< Iterator over vector values + typedef Values::const_iterator const_iterator; ///< Const iterator over vector values + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Values::value_type value_type; ///< Typedef to pair + typedef value_type KeyValuePair; ///< Typedef to pair + typedef std::map Dims; ///< Keyed vector dimensions /// @name Standard Constructors /// @{ @@ -111,7 +102,8 @@ namespace gtsam { */ VectorValues() {} - /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ + /** Merge two VectorValues into one, this is more efficient than inserting + * elements one by one. */ VectorValues(const VectorValues& first, const VectorValues& second); /** Create from another container holding pair. */ @@ -147,20 +139,26 @@ namespace gtsam { /** Check whether a variable with key \c j exists. */ bool exists(Key j) const { return find(j) != end(); } - /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + /** + * Read/write access to the vector value with key \c j, throws + * std::out_of_range if \c j does not exist, identical to operator[](Key). + */ Vector& at(Key j) { iterator item = find(j); - if(item == end()) + if (item == end()) throw std::out_of_range( "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); else return item->second; } - /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + /** + * Access the vector value with key \c j (const version), throws + * std::out_of_range if \c j does not exist, identical to operator[](Key). + */ const Vector& at(Key j) const { const_iterator item = find(j); - if(item == end()) + if (item == end()) throw std::out_of_range( "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); else @@ -207,26 +205,30 @@ namespace gtsam { /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) { - if(values_.unsafe_erase(var) == 0) - throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); + if (values_.unsafe_erase(var) == 0) + throw std::invalid_argument("Requested variable '" + + DefaultKeyFormatter(var) + + "', is not in this VectorValues."); } /** Set all values to zero vectors. */ void setZero(); - iterator begin() { return values_.begin(); } ///< Iterator over variables - const_iterator begin() const { return values_.begin(); } ///< Iterator over variables - iterator end() { return values_.end(); } ///< Iterator over variables - const_iterator end() const { return values_.end(); } ///< Iterator over variables - //reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables - //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - //reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables - //const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + iterator begin() { return values_.begin(); } ///< Iterator over variables + const_iterator begin() const { return values_.begin(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables + const_iterator end() const { return values_.end(); } ///< Iterator over variables - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + /** + * Return the iterator corresponding to the requested key, or end() if no + * variable is present with this key. + */ iterator find(Key j) { return values_.find(j); } - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + /** + * Return the iterator corresponding to the requested key, or end() if no + * variable is present with this key. + */ const_iterator find(Key j) const { return values_.find(j); } /** print required by Testable for unit testing */ @@ -244,7 +246,26 @@ namespace gtsam { Vector vector() const; /** Access a vector that is a subset of relevant keys. */ - Vector vector(const FastVector& keys) const; + template + Vector vector(const CONTAINER& keys) const { + DenseIndex totalDim = 0; + FastVector items; + items.reserve(keys.end() - keys.begin()); + for (Key key : keys) { + const Vector* v = &at(key); + totalDim += v->size(); + items.push_back(v); + } + + Vector result(totalDim); + DenseIndex pos = 0; + for (const Vector* v : items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } + + return result; + } /** Access a vector that is a subset of relevant keys, dims version. */ Vector vector(const Dims& dims) const; @@ -319,54 +340,6 @@ namespace gtsam { /// @} - /** - * scale a vector by a scalar - */ - //friend VectorValues operator*(const double a, const VectorValues &v) { - // VectorValues result = VectorValues::SameStructure(v); - // for(Key j = 0; j < v.size(); ++j) - // result.values_[j] = a * v.values_[j]; - // return result; - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // y.values_[j] += alpha * x.values_[j]; - // else - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - //} - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void sqrt(VectorValues &x) { - // for(Key j = 0; j < x.size(); ++j) - // x.values_[j] = x.values_[j].cwiseSqrt(); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { - // if(numerator.size() != denominator.size() || numerator.size() != result.size()) - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < numerator.size(); ++j) - // if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) - // result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); - // else - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void edivInPlace(VectorValues& x, const VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // x.values_[j].array() /= y.values_[j].array(); - // else - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - //} - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 55f043dd3..ccf5db5c3 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -106,6 +106,10 @@ public: preintMeasCov_(preintMeasCov) { } + /// Virtual destructor + virtual ~PreintegratedImuMeasurements() { + } + /// print void print(const std::string& s = "Preintegrated Measurements:") const override; diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 34c38e22b..da49e4ddd 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -42,7 +42,7 @@ class TangentPreintegration : public PreintegrationBase { } public: - /// @name Constructors + /// @name Constructors/destructors /// @{ /** @@ -53,6 +53,10 @@ public: TangentPreintegration(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + /// Virtual destructor + virtual ~TangentPreintegration() { + } + /// @} /// @name Basic utilities diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index a0fc117d9..8a8813af5 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -11,120 +11,89 @@ /** * @file ISAM2-impl.cpp - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess * @author Richard Roberts */ -#include -#include // for selective linearization thresholds #include -#include // for GTSAM_USE_TBB +#include // for GTSAM_USE_TBB +#include // for selective linearization thresholds +#include -#include #include +#include +#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter) -{ - const bool debug = ISDEBUG("ISAM2 AddVariables"); +void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, + bool useUnusedSlots, + NonlinearFactorGraph* nonlinearFactors, + FactorIndices* newFactorIndices) { + newFactorIndices->resize(newFactors.size()); - theta.insert(newTheta); - if(debug) newTheta.print("The new variables are: "); - // Add zeros into the VectorValues - delta.insert(newTheta.zeroVectors()); - deltaNewton.insert(newTheta.zeroVectors()); - RgProd.insert(newTheta.zeroVectors()); -} - -/* ************************************************************************* */ -void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices) -{ - newFactorIndices.resize(newFactors.size()); - - if(useUnusedSlots) - { + if (useUnusedSlots) { size_t globalFactorIndex = 0; - for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) - { + for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); + ++newFactorIndex) { // Loop to find the next available factor slot - do - { - // If we need to add more factors than we have room for, resize nonlinearFactors, - // filling the new slots with NULL factors. Otherwise, check if the current - // factor in nonlinearFactors is already used, and if so, increase - // globalFactorIndex. If the current factor in nonlinearFactors is unused, break - // out of the loop and use the current slot. - if(globalFactorIndex >= nonlinearFactors.size()) - nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex); - else if(nonlinearFactors[globalFactorIndex]) - ++ globalFactorIndex; + do { + // If we need to add more factors than we have room for, resize + // nonlinearFactors, filling the new slots with NULL factors. Otherwise, + // check if the current factor in nonlinearFactors is already used, and + // if so, increase globalFactorIndex. If the current factor in + // nonlinearFactors is unused, break out of the loop and use the current + // slot. + if (globalFactorIndex >= nonlinearFactors->size()) + nonlinearFactors->resize(nonlinearFactors->size() + + newFactors.size() - newFactorIndex); + else if ((*nonlinearFactors)[globalFactorIndex]) + ++globalFactorIndex; else break; - } while(true); + } while (true); // Use the current slot, updating nonlinearFactors and newFactorSlots. - nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; - newFactorIndices[newFactorIndex] = globalFactorIndex; + (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; + (*newFactorIndices)[newFactorIndex] = globalFactorIndex; } - } - else - { + } else { // We're not looking for unused slots, so just add the factors at the end. - for(size_t i = 0; i < newFactors.size(); ++i) - newFactorIndices[i] = i + nonlinearFactors.size(); - nonlinearFactors.push_back(newFactors); + for (size_t i = 0; i < newFactors.size(); ++i) + (*newFactorIndices)[i] = i + nonlinearFactors->size(); + nonlinearFactors->push_back(newFactors); } } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, - Values& theta, VariableIndex& variableIndex, - VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables) -{ - variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); - for(Key key: unusedKeys) { - delta.erase(key); - deltaNewton.erase(key); - RgProd.erase(key); - replacedKeys.erase(key); - nodes.unsafe_erase(key); - theta.erase(key); - fixedVariables.erase(key); - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) -{ +KeySet ISAM2::Impl::CheckRelinearizationFull( + const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; - if(const double* threshold = boost::get(&relinearizeThreshold)) - { - for(const VectorValues::KeyValuePair& key_delta: delta) { + if (const double* threshold = boost::get(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { double maxDelta = key_delta.second.lpNorm(); - if(maxDelta >= *threshold) - relinKeys.insert(key_delta.first); + if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); } - } - else if(const FastMap* thresholds = boost::get >(&relinearizeThreshold)) - { - for(const VectorValues::KeyValuePair& key_delta: delta) { - const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; - if(threshold.rows() != key_delta.second.rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); - if((key_delta.second.array().abs() > threshold.array()).any()) + } else if (const FastMap* thresholds = + boost::get >(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + const Vector& threshold = + thresholds->find(Symbol(key_delta.first).chr())->second; + if (threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(key_delta.first).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + if ((key_delta.second.array().abs() > threshold.array()).any()) relinKeys.insert(key_delta.first); } } @@ -133,167 +102,117 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, - const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) -{ +static void CheckRelinearizationRecursiveDouble( + double threshold, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { // Check the current clique for relinearization bool relinearize = false; - for(Key var: *clique->conditional()) { + for (Key var : *clique->conditional()) { double maxDelta = delta[var].lpNorm(); - if(maxDelta >= threshold) { - relinKeys.insert(var); + if (maxDelta >= threshold) { + relinKeys->insert(var); relinearize = true; } } // If this node was relinearized, also check its children - if(relinearize) { - for(const ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); } } } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap& thresholds, - const VectorValues& delta, - const ISAM2Clique::shared_ptr& clique) -{ +static void CheckRelinearizationRecursiveMap( + const FastMap& thresholds, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { // Check the current clique for relinearization bool relinearize = false; - for(Key var: *clique->conditional()) { + for (Key var : *clique->conditional()) { // Find the threshold for this variable type const Vector& threshold = thresholds.find(Symbol(var).chr())->second; const Vector& deltaVar = delta[var]; // Verify the threshold vector matches the actual variable size - if(threshold.rows() != deltaVar.rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(var).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); + if (threshold.rows() != deltaVar.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(var).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); // Check for relinearization - if((deltaVar.array().abs() > threshold.array()).any()) { - relinKeys.insert(var); + if ((deltaVar.array().abs() > threshold.array()).any()) { + relinKeys->insert(var); relinearize = true; } } // If this node was relinearized, also check its children - if(relinearize) { - for(const ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); } } } /* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, - const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) -{ +KeySet ISAM2::Impl::CheckRelinearizationPartial( + const ISAM2::Roots& roots, const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; - for(const ISAM2::sharedClique& root: roots) { - if(relinearizeThreshold.type() == typeid(double)) - CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); - else if(relinearizeThreshold.type() == typeid(FastMap)) - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, root); + for (const ISAM2::sharedClique& root : roots) { + if (relinearizeThreshold.type() == typeid(double)) + CheckRelinearizationRecursiveDouble( + boost::get(relinearizeThreshold), delta, root, &relinKeys); + else if (relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap( + boost::get >(relinearizeThreshold), delta, root, + &relinKeys); } return relinKeys; } -/* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask) -{ - static const bool debug = false; - // does the separator contain any of the variables? - bool found = false; - for(Key key: clique->conditional()->parents()) { - if (markedMask.exists(key)) { - found = true; - break; - } - } - if (found) { - // then add this clique - keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); - if(debug) clique->print("Key(s) marked in clique "); - if(debug) cout << "so marking key " << clique->conditional()->front() << endl; - } - for(const ISAM2Clique::shared_ptr& child: clique->children) { - FindAll(child, keys, markedMask); - } -} - -/* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) -{ - // If debugging, invalidate if requested, otherwise do not invalidate. - // Invalidating means setting expmapped entries to Inf, to trigger assertions - // if we try to re-use them. -#ifdef NDEBUG - invalidateIfDebug = boost::none; -#endif - - assert(values.size() == delta.size()); - Values::iterator key_value; - VectorValues::const_iterator key_delta; -#ifdef GTSAM_USE_TBB - for(key_value = values.begin(); key_value != values.end(); ++key_value) - { - key_delta = delta.find(key_value->key); -#else - for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta) - { - assert(key_value->key == key_delta->first); -#endif - Key var = key_value->key; - assert(delta[var].size() == (int)key_value->value.dim()); - assert(delta[var].allFinite()); - if(mask.exists(var)) { - Value* retracted = key_value->value.retract_(delta[var]); - key_value->value = *retracted; - retracted->deallocate_(); - if(invalidateIfDebug) - (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) - } - } -} - /* ************************************************************************* */ namespace internal { -inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { +inline static void optimizeInPlace(const ISAM2::sharedClique& clique, + VectorValues* result) { // parents are assumed to already be solved and available in result - result.update(clique->conditional()->solve(result)); + result->update(clique->conditional()->solve(*result)); // starting from the root, call optimize on each conditional - for(const boost::shared_ptr& child: clique->children) + for (const ISAM2::sharedClique& child : clique->children) optimizeInPlace(child, result); } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { - +size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + double wildfireThreshold, + VectorValues* delta) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - for(const ISAM2::sharedClique& root: roots) + for (const ISAM2::sharedClique& root : roots) internal::optimizeInPlace(root, delta); - lastBacksubVariableCount = delta.size(); + lastBacksubVariableCount = delta->size(); } else { // Optimize with wildfire lastBacksubVariableCount = 0; - for(const ISAM2::sharedClique& root: roots) + for (const ISAM2::sharedClique& root : roots) lastBacksubVariableCount += optimizeWildfireNonRecursive( - root, wildfireThreshold, replacedKeys, delta); // modifies delta + root, wildfireThreshold, replacedKeys, delta); // modifies delta -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(size_t j=0; j)).all()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (VectorValues::const_iterator key_delta = delta->begin(); + key_delta != delta->end(); ++key_delta) { + assert((*delta)[key_delta->first].allFinite()); + } #endif } @@ -302,69 +221,77 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector /* ************************************************************************* */ namespace internal { -void updateRgProd(const boost::shared_ptr& clique, const KeySet& replacedKeys, - const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { - +void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, + const VectorValues& grad, VectorValues* RgProd, + size_t* varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - for(Key j: *clique->conditional()) { - if(replacedKeys.exists(j)) { + for (Key j : *clique->conditional()) { + if (replacedKeys.exists(j)) { anyReplaced = true; break; } } - if(anyReplaced) { + if (anyReplaced) { // Update the current variable // Get VectorValues slice corresponding to current variables - Vector gR = grad.vector(FastVector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); - Vector gS = grad.vector(FastVector(clique->conditional()->beginParents(), clique->conditional()->endParents())); + Vector gR = + grad.vector(FastVector(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals())); + Vector gS = + grad.vector(FastVector(clique->conditional()->beginParents(), + clique->conditional()->endParents())); // Compute R*g and S*g for this clique - Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; + Vector RSgProd = clique->conditional()->get_R() * gR + + clique->conditional()->get_S() * gS; // Write into RgProd vector DenseIndex vectorPosition = 0; - for(Key frontal: clique->conditional()->frontals()) { - Vector& RgProdValue = RgProd[frontal]; + for (Key frontal : clique->conditional()->frontals()) { + Vector& RgProdValue = (*RgProd)[frontal]; RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); vectorPosition += RgProdValue.size(); } - // Now solve the part of the Newton's method point for this clique (back-substitution) - //(*clique)->solveInPlace(deltaNewton); + // Now solve the part of the Newton's method point for this clique + // (back-substitution) + // (*clique)->solveInPlace(deltaNewton); - varsUpdated += clique->conditional()->nrFrontals(); + *varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - for(const ISAM2Clique::shared_ptr& child: clique->children) { - updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } + for (const ISAM2::sharedClique& child : clique->children) { + updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); + } } } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues& RgProd) { - +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + const VectorValues& gradAtZero, + VectorValues* RgProd) { // Update variables size_t varsUpdated = 0; - for(const ISAM2::sharedClique& root: roots) { - internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); + for (const ISAM2::sharedClique& root : roots) { + internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, + &varsUpdated); } return varsUpdated; } - + /* ************************************************************************* */ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, - const VectorValues& RgProd) -{ + const VectorValues& RgProd) { // Compute gradient squared-magnitude const double gradientSqNorm = gradAtZero.dot(gradAtZero); - + // Compute minimizing step size double RgNormSq = RgProd.vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; @@ -373,4 +300,4 @@ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, return step * gradAtZero; } -} +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 161932344..8a30fb8cd 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -23,7 +23,6 @@ namespace gtsam { struct GTSAM_EXPORT ISAM2::Impl { - struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; }; @@ -32,35 +31,14 @@ struct GTSAM_EXPORT ISAM2::Impl { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional > constrainedKeys; + boost::optional > constrainedKeys; }; - /** - * Add new variables to the ISAM2 system. - * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param ordering Current ordering to be augmented with new variables - * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the /// complete list of nonlinear factors, and populates the list of new factor indices, both /// optionally finding and reusing empty factor slots. static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices); - - /** - * Remove variables from the ISAM2 system. - */ - static void RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, - Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables); + NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -85,57 +63,22 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static KeySet CheckRelinearizationPartial(const FastVector& roots, + static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - /** - * Recursively search this clique and its children for marked keys appearing - * in the separator, and add the *frontal* keys of any cliques whose - * separator contains any marked keys to the set \c keys. The purpose of - * this is to discover the cliques that need to be redone due to information - * propagating to them from cliques that directly contain factors being - * relinearized. - * - * The original comment says this finds all variables directly connected to - * the marked ones by measurements. Is this true, because it seems like this - * would also pull in variables indirectly connected through other frontal or - * separator variables? - * - * Alternatively could we trace up towards the root for each variable here? - */ - static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask); - - /** - * Apply expmap to the given values, but only for indices appearing in - * \c markedRelinMask. Values are expmapped in-place. - * \param [in, out] values The value to expmap in-place - * \param delta The linear delta with which to expmap - * \param ordering The ordering - * \param mask Mask on linear indices, only \c true entries are expmapped - * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, - * expmapped deltas will be set to an invalid value (infinity) to catch bugs - * where we might expmap something twice, or expmap it but then not - * recalculate its delta. - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, - boost::optional invalidateIfDebug = boost::none, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /** * Update the Newton's method step point, using wildfire */ - static size_t UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold); + static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, + const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues& RgProd); - + const VectorValues& gradAtZero, VectorValues* RgProd); + /** * Compute the gradient-search point. Only used in Dogleg. */ diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h deleted file mode 100644 index f04e16b7d..000000000 --- a/gtsam/nonlinear/ISAM2-inl.h +++ /dev/null @@ -1,311 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file ISAM2-inl.h - * @brief - * @author Richard Roberts - * @date Mar 16, 2012 - */ - - -#pragma once - -#include -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Vector& delta = getDelta()[key]; - return traits::Retract(theta_.at(key), delta); -} - -/* ************************************************************************* */ -namespace internal { -template -void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - if(recalculate) { - - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - delta.update(clique->conditional()->solve(delta)); - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - - // Recurse to children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - optimizeWildfire(child, threshold, changed, replaced, delta, count); - } - } -} - -template -bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, potentially refactor - if(recalculate) - { - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - special version stores solution pointers in cliques for fast access. - { - // Create solution part pointers if necessary and possible - necessary if solnPointers_ is - // empty, and possible if either we're a root, or we have a parent with valid solnPointers_. - boost::shared_ptr parent = clique->parent_.lock(); - if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) - { - for(Key key: clique->conditional()->frontals()) - clique->solnPointers_.insert(std::make_pair(key, delta.find(key))); - for(Key key: clique->conditional()->parents()) - clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key))); - } - - // See if we can use solution part pointers - we can if they either already existed or were - // created above. - if(!clique->solnPointers_.empty()) - { - GaussianConditional& c = *clique->conditional(); - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(clique->conditional()->nrParents()); - for(Key parent: clique->conditional()->parents()) { - parentPointers.push_back(clique->solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - for(const VectorValues::const_iterator& parentPointer: parentPointers) { - const Vector& parentVector = parentPointer->second; - xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); - vectorPos += parentVector.size(); - } - } - - // NOTE(gareth): We can no longer write: xS = b - S * xS - // This is because Eigen (as of 3.3) no longer evaluates S * xS into - // a temporary, and the operation trashes valus in xS. - // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = c.get_R().triangularView().solve(rhs); - - // Check for indeterminant solution - if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - clique->solnPointers_.at(*frontal)->second = solution.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); - } - } - else - { - // Just call plain solve because we couldn't use solution pointers. - delta.update(clique->conditional()->solve(delta)); - } - } - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - } - - return recalculate; -} - -} // namespace internal - -/* ************************************************************************* */ -template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { - KeySet changed; - int count = 0; - // starting from the root, call optimize on each conditional - if(root) - internal::optimizeWildfire(root, threshold, changed, keys, delta, count); - return count; -} - -/* ************************************************************************* */ -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) -{ - KeySet changed; - size_t count = 0; - - if (root) { - std::stack > travStack; - travStack.push(root); - boost::shared_ptr currentNode = root; - while (!travStack.empty()) { - currentNode = travStack.top(); - travStack.pop(); - bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); - if (recalculate) { - for(const typename CLIQUE::shared_ptr& child: currentNode->children) { - travStack.push(child); - } - } - } - } - - return count; -} - -/* ************************************************************************* */ -template -void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (int)clique->conditional()->rows(); - int dimSep = (int)clique->conditional()->get_S().cols(); - result += ((dimR+1)*dimR)/2 + dimSep*dimR; - // traverse the children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - nnz_internal(child, result); - } -} - -/* ************************************************************************* */ -template -int calculate_nnz(const boost::shared_ptr& clique) { - int result = 0; - // starting from the root, add up entries of frontal and conditional matrices of each conditional - nnz_internal(clique, result); - return result; -} - -} - diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 122bf8aeb..df07050de 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -10,50 +10,48 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-inl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @file ISAM2.cpp + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ -#include // for operator += -using namespace boost::assign; +#include + +#include +#include +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include +#include + #include #include -#include -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} // namespace br -#include -#include -#include -#include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include -#include -#include -#include - -#include -#include -#include -#include +#include +#include +#include +#include using namespace std; namespace gtsam { -// Instantiate base classes -template class BayesTreeCliqueBase; +// Instantiate base class template class BayesTree; -static const bool disableReordering = false; -static const double batchThreshold = 0.65; +static const bool kDisableReordering = false; +static const double kBatchThreshold = 0.65; /* ************************************************************************* */ -// Special BayesTree class that uses ISAM2 cliques - this is the result of reeliminating ISAM2 -// subtrees. -class ISAM2BayesTree : public ISAM2::Base -{ -public: +// Special BayesTree class that uses ISAM2 cliques - this is the result of +// reeliminating ISAM2 subtrees. +class ISAM2BayesTree : public ISAM2::Base { + public: typedef ISAM2::Base Base; typedef ISAM2BayesTree This; typedef boost::shared_ptr shared_ptr; @@ -62,129 +60,65 @@ public: }; /* ************************************************************************* */ -// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for reeliminating ISAM2 -// subtrees. -class ISAM2JunctionTree : public JunctionTree -{ -public: +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for +// reeliminating ISAM2 subtrees. +class ISAM2JunctionTree + : public JunctionTree { + public: typedef JunctionTree Base; typedef ISAM2JunctionTree This; typedef boost::shared_ptr shared_ptr; - ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : - Base(eliminationTree) {} + explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + : Base(eliminationTree) {} }; /* ************************************************************************* */ -std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { - std::string s; - switch (adaptationMode) { - case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: s = "SEARCH_EACH_ITERATION"; break; - case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: s = "ONE_STEP_PER_ITERATION"; break; - default: s = "UNDEFINED"; break; - } - return s; -} - -/* ************************************************************************* */ -DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator(const std::string& adaptationMode) const { - std::string s = adaptationMode; boost::algorithm::to_upper(s); - if (s == "SEARCH_EACH_ITERATION") return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; - if (s == "ONE_STEP_PER_ITERATION") return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; - - /* default is SEARCH_EACH_ITERATION */ - return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; -} - -/* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { - std::string s = str; boost::algorithm::to_upper(s); - if (s == "QR") return ISAM2Params::QR; - if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; - - /* default is CHOLESKY */ - return ISAM2Params::CHOLESKY; -} - -/* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { - std::string s; - switch (value) { - case ISAM2Params::QR: s = "QR"; break; - case ISAM2Params::CHOLESKY: s = "CHOLESKY"; break; - default: s = "UNDEFINED"; break; - } - return s; -} - -/* ************************************************************************* */ -void ISAM2Clique::setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult) -{ - conditional_ = eliminationResult.first; - cachedFactor_ = eliminationResult.second; - // Compute gradient contribution - gradientContribution_.resize(conditional_->cols() - 1); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons - gradientContribution_ << -conditional_->get_R().transpose() * conditional_->get_d(), - -conditional_->get_S().transpose() * conditional_->get_d(); -} - -/* ************************************************************************* */ -bool ISAM2Clique::equals(const This& other, double tol) const { - return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) - || (cachedFactor_ && other.cachedFactor_ - && cachedFactor_->equals(*other.cachedFactor_, tol))); -} - -/* ************************************************************************* */ -void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) const -{ - Base::print(s,formatter); - if(cachedFactor_) - cachedFactor_->print(s + "Cached: ", formatter); - else - std::cout << s << "Cached empty" << std::endl; - if(gradientContribution_.rows() != 0) - gtsam::print(gradientContribution_, "Gradient contribution: "); -} - -/* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2Params& params): params_(params), update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ ISAM2::ISAM2() : update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ bool ISAM2::equals(const ISAM2& other, double tol) const { - return Base::equals(other, tol) - && theta_.equals(other.theta_, tol) && variableIndex_.equals(other.variableIndex_, tol) - && nonlinearFactors_.equals(other.nonlinearFactors_, tol) - && fixedVariables_ == other.fixedVariables_; + return Base::equals(other, tol) && theta_.equals(other.theta_, tol) && + variableIndex_.equals(other.variableIndex_, tol) && + nonlinearFactors_.equals(other.nonlinearFactors_, tol) && + fixedVariables_ == other.fixedVariables_; } /* ************************************************************************* */ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; - if(debug) cout << "Getting affected factors for "; - if(debug) { for(const Key key: keys) { cout << key << " "; } } - if(debug) cout << endl; + if (debug) cout << "Getting affected factors for "; + if (debug) { + for (const Key key : keys) { + cout << key << " "; + } + } + if (debug) cout << endl; NonlinearFactorGraph allAffected; KeySet indices; - for(const Key key: keys) { + for (const Key key : keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } - if(debug) cout << "Affected factors are: "; - if(debug) { for(const size_t index: indices) { cout << index << " "; } } - if(debug) cout << endl; + if (debug) cout << "Affected factors are: "; + if (debug) { + for (const size_t index : indices) { + cout << index << " "; + } + } + if (debug) cout << endl; return indices; } @@ -192,9 +126,8 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) -GaussianFactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const -{ +GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); @@ -208,29 +141,29 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - for(Key idx: candidates) { + auto linearized = boost::make_shared(); + for (Key idx : candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; - for(Key key: nonlinearFactors_[idx]->keys()) { - if(affectedKeysSet.find(key) == affectedKeysSet.end()) { + for (Key key : nonlinearFactors_[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { inside = false; break; } - if(useCachedLinear && relinKeys.find(key) != relinKeys.end()) + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) useCachedLinear = false; } - if(inside) { - if(useCachedLinear) { + if (inside) { + if (useCachedLinear) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]); assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); #endif linearized->push_back(linearFactors_[idx]); } else { - GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_); + auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); linearized->push_back(linearFactor); - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]->keys() == linearFactor->keys()); #endif @@ -245,12 +178,13 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the affected area +// find intermediate (linearized) factors from cache that are passed into the +// affected area -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { GaussianFactorGraph cachedBoundary; - for(sharedClique orphan: orphans) { + for (sharedClique orphan : orphans) { // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); } @@ -259,27 +193,27 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } /* ************************************************************************* */ -boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const vector& observedKeys, - const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result& result) -{ - // TODO: new factors are linearized twice, the newFactors passed in are not used. +boost::shared_ptr ISAM2::recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const vector& observedKeys, const KeySet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result* result) { + // TODO(dellaert): new factors are linearized twice, + // the newFactors passed in are not used. const bool debug = ISDEBUG("ISAM2 recalculate"); // Input: BayesTree(this), newFactors -//#define PRINT_STATS // figures for paper, disable for timing +// figures for paper, disable for timing #ifdef PRINT_STATS static int counter = 0; int maxClique = 0; double avgClique = 0; int numCliques = 0; int nnzR = 0; - if (counter>0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); + if (counter > 0) { // cannot call on empty tree + GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); maxClique = cstats.maxCONDITIONALSize; avgClique = cstats.avgCONDITIONALSize; @@ -289,81 +223,89 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke counter++; #endif - if(debug) { + if (debug) { cout << "markedKeys: "; - for(const Key key: markedKeys) { cout << key << " "; } + for (const Key key : markedKeys) { + cout << key << " "; + } cout << endl; cout << "observedKeys: "; - for(const Key key: observedKeys) { cout << key << " "; } + for (const Key key : observedKeys) { + cout << key << " "; + } cout << endl; } // 1. Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all parents up to the root. - // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. gttic(removetop); Cliques orphans; GaussianBayesNet affectedBayesNet; - this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); + this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), + affectedBayesNet, orphans); gttoc(removetop); // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached factors get messed up - // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't - // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose - // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected - // in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary + // bug was here: we cannot reuse the original factors, because then the cached + // factors get messed up [all the necessary data is actually contained in the + // affectedBayesNet, including what was passed in from the boundaries, + // so this would be correct; however, in the process we also generate new + // cached_ entries that will be wrong (ie. they don't contain what would be + // passed up at a certain point if batch elimination was done, but that's + // what we need); we could choose not to update cached_ from here, but then + // the new information (and potentially different variable ordering) is not + // reflected in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add the + // cached factors from the boundary // BEGIN OF COPIED CODE - // ordering provides all keys in conditionals, there cannot be others because path to root included + // ordering provides all keys in conditionals, there cannot be others because + // path to root included gttic(affectedKeys); FastList affectedKeys; - for(const ConditionalType::shared_ptr& conditional: affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr affectedKeysSet(new KeySet()); // Will return this result + boost::shared_ptr affectedKeysSet( + new KeySet()); // Will return this result - if(affectedKeys.size() >= theta_.size() * batchThreshold) - { + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { // Do a batch step - reorder and relinearize all variables gttic(batch); gttic(add_keys); - br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end())); + br::copy(variableIndex_ | br::map_keys, + std::inserter(*affectedKeysSet, affectedKeysSet->end())); // Removed unused keys: VariableIndex affectedFactorsVarIndex = variableIndex_; - affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), unusedIndices.end()); + affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), + unusedIndices.end()); - for (const Key key: unusedIndices) - { + for (const Key key : unusedIndices) { affectedKeysSet->erase(key); } gttoc(add_keys); gttic(ordering); Ordering order; - if(constrainKeys) - { - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); - } - else - { - if(theta_.size() > observedKeys.size()) - { + if (constrainKeys) { + order = + Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); + } else { + if (theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained FastMap constraintGroups; - for(Key var: observedKeys) - constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); - } - else - { + for (Key var : observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { order = Ordering::Colamd(affectedFactorsVarIndex); } } @@ -371,95 +313,110 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(linearize); GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); - if(params_.cacheLinearizedFactors) - linearFactors_ = linearized; + if (params_.cacheLinearizedFactors) linearFactors_ = linearized; gttoc(linearize); gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()).first; + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; gttoc(eliminate); gttic(insert); this->clear(); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(insert); - result.variablesReeliminated = affectedKeysSet->size(); - result.factorsRecalculated = nonlinearFactors_.size(); + result->variablesReeliminated = affectedKeysSet->size(); + result->factorsRecalculated = nonlinearFactors_.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); lastAffectedFactorCount = linearized.size(); // Reeliminated keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: theta_.keys()) { - result.detail->variableStatus[key].isReeliminated = true; + if (params_.enableDetailedResults) { + for (Key key : theta_.keys()) { + result->detail->variableStatus[key].isReeliminated = true; } } gttoc(batch); } else { - gttic(incremental); // 2. Add the new factors \Factors' into the resulting factor graph FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), + observedKeys.end()); gttic(relinearizeAffected); - GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); - if(debug) factors.print("Relinearized factors: "); + GaussianFactorGraph factors( + *relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); + if (debug) factors.print("Relinearized factors: "); gttoc(relinearizeAffected); - if(debug) { cout << "Affected keys: "; for(const Key key: affectedKeys) { cout << key << " "; } cout << endl; } + if (debug) { + cout << "Affected keys: "; + for (const Key key : affectedKeys) { + cout << key << " "; + } + cout << endl; + } // Reeliminated keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: affectedAndNewKeys) { - result.detail->variableStatus[key].isReeliminated = true; + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; } } - result.variablesReeliminated = affectedAndNewKeys.size(); - result.factorsRecalculated = factors.size(); + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeys.size(); lastAffectedFactorCount = factors.size(); #ifdef PRINT_STATS // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique - << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; + cout << "linear: #markedKeys: " << markedKeys.size() + << " #affectedVariables: " << affectedKeys.size() + << " #affectedFactors: " << factors.size() + << " maxCliqueSize: " << maxClique << " avgCliqueSize: " << avgClique + << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; #endif gttic(cached); // add the cached intermediate results from the boundary of the orphans ... GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if(debug) cachedBoundary.print("Boundary factors: "); + if (debug) cachedBoundary.print("Boundary factors: "); factors.push_back(cachedBoundary); gttoc(cached); gttic(orphans); // Add the orphaned subtrees - for(const sharedClique& orphan: orphans) + for (const sharedClique& orphan : orphans) factors += boost::make_shared >(orphan); gttoc(orphans); - // END OF COPIED CODE - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) gttic(reorder_and_eliminate); gttic(list_to_set); // create a partial reordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in the ordering + // markedKeys are passed in: those variables will be forced to the end in + // the ordering affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); gttoc(list_to_set); @@ -468,37 +425,46 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(ordering_constraints); // Create ordering constraints - FastMap constraintGroups; - if(constrainKeys) { + FastMap constraintGroups; + if (constrainKeys) { constraintGroups = *constrainKeys; } else { - constraintGroups = FastMap(); - const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var: observedKeys) + constraintGroups = FastMap(); + const int group = + observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + for (Key var : observedKeys) constraintGroups.insert(make_pair(var, group)); } // Remove unaffected keys from the constraints - for(FastMap::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); /*Incremented in loop ++iter*/) { - if(unusedIndices.exists(iter->first) || !affectedKeysSet->exists(iter->first)) - constraintGroups.erase(iter ++); + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (unusedIndices.exists(iter->first) || + !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter++); else - ++ iter; + ++iter; } gttoc(ordering_constraints); // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = + Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( - factors, affectedFactorsVarIndex, ordering)).eliminate(params_.getEliminationFunction()).first; + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) + .eliminate(params_.getEliminationFunction()) + .first; gttoc(reorder_and_eliminate); gttic(reassemble); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(reassemble); @@ -508,22 +474,79 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke } // Root clique variables for detailed results - if(params_.enableDetailedResults) { - for(const sharedNode& root: this->roots()) - for(Key var: *root->conditional()) - result.detail->variableStatus[var].inRootClique = true; + if (params_.enableDetailedResults) { + for (const sharedNode& root : this->roots()) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; } return affectedKeysSet; } /* ************************************************************************* */ -ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, - const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, - const boost::optional >& extraReelimKeys, bool force_relinearize) -{ +void ISAM2::addVariables(const Values& newTheta) { + const bool debug = ISDEBUG("ISAM2 AddVariables"); + theta_.insert(newTheta); + if (debug) newTheta.print("The new variables are: "); + // Add zeros into the VectorValues + delta_.insert(newTheta.zeroVectors()); + deltaNewton_.insert(newTheta.zeroVectors()); + RgProd_.insert(newTheta.zeroVectors()); +} + +/* ************************************************************************* */ +void ISAM2::removeVariables(const KeySet& unusedKeys) { + variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); + for (Key key : unusedKeys) { + delta_.erase(key); + deltaNewton_.erase(key); + RgProd_.erase(key); + deltaReplacedMask_.erase(key); + Base::nodes_.unsafe_erase(key); + theta_.erase(key); + fixedVariables_.erase(key); + } +} + +/* ************************************************************************* */ +void ISAM2::expmapMasked(const KeySet& mask) { + assert(theta_.size() == delta_.size()); + Values::iterator key_value; + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) { + key_delta = delta_.find(key_value->key); +#else + for (key_value = theta_.begin(), key_delta = delta_.begin(); + key_value != theta_.end(); ++key_value, ++key_delta) { + assert(key_value->key == key_delta->first); +#endif + Key var = key_value->key; + assert(static_cast(delta_[var].size()) == key_value->value.dim()); + assert(delta_[var].allFinite()); + if (mask.exists(var)) { + Value* retracted = key_value->value.retract_(delta_[var]); + key_value->value = *retracted; + retracted->deallocate_(); +#ifndef NDEBUG + // If debugging, invalidate delta_ entries to Inf, to trigger assertions + // if we try to re-use them. + delta_[var] = Vector::Constant(delta_[var].rows(), + numeric_limits::infinity()); +#endif + } + } +} + +/* ************************************************************************* */ +ISAM2Result ISAM2::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const FactorIndices& removeFactorIndices, + const boost::optional >& constrainedKeys, + const boost::optional >& noRelinKeys, + const boost::optional >& extraReelimKeys, + bool force_relinearize) { const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -538,40 +561,44 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; - if(params_.enableDetailedResults) + if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); - const bool relinearizeThisStep = force_relinearize - || (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); + const bool relinearizeThisStep = + force_relinearize || (params_.enableRelinearization && + update_count_ % params_.relinearizeSkip == 0); - if(verbose) { + if (verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); } // Update delta if we need it to check relinearization later - if(relinearizeThisStep) { + if (relinearizeThisStep) { gttic(updateDelta); - updateDelta(disableReordering); + updateDelta(kDisableReordering); gttoc(updateDelta); } gttic(push_back_factors); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. // Add the new factor indices to the result struct - if(debug || verbose) newFactors.print("The new factors are: "); - Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors_, result.newFactorsIndices); + if (debug || verbose) newFactors.print("The new factors are: "); + Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, + &nonlinearFactors_, &result.newFactorsIndices); // Remove the removed factors - NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for(size_t index: removeFactorIndices) { + NonlinearFactorGraph removeFactors; + removeFactors.reserve(removeFactorIndices.size()); + for (size_t index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); - if(params_.cacheLinearizedFactors) - linearFactors_.remove(index); + if (params_.cacheLinearizedFactors) linearFactors_.remove(index); } - // Remove removed factors from the variable index so we do not attempt to relinearize them - variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); + // Remove removed factors from the variable index so we do not attempt to + // relinearize them + variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), + removeFactors); // Compute unused keys and indices KeySet unusedKeys; @@ -580,123 +607,144 @@ ISAM2Result ISAM2::update( // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. KeySet removedAndEmpty; - for(Key key: removeFactors.keys()) { - if(variableIndex_[key].empty()) + for (Key key : removeFactors.keys()) { + if (variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } KeySet newFactorSymbKeys = newFactors.keys(); std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(unusedKeys, unusedKeys.end())); // Get indices for unused keys - for(Key key: unusedKeys) { + for (Key key : unusedKeys) { unusedIndices.insert(unusedIndices.end(), key); } } gttoc(push_back_factors); gttic(add_new_variables); - // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); + // 2. Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. + addVariables(newTheta); // New keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } + if (params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + result.detail->variableStatus[key].isNew = true; + } + } gttoc(add_new_variables); gttic(evaluate_error_before); - if(params_.evaluateNonlinearError) + if (params_.evaluateNonlinearError) result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_before); gttic(gather_involved_keys); // 3. Mark linear update - KeySet markedKeys = newFactors.keys(); // Get keys from new factors + KeySet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors - markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys + KeySet markedRemoveKeys = + removeFactors.keys(); // Get keys involved in removed factors + markedKeys.insert( + markedRemoveKeys.begin(), + markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys - if(extraReelimKeys) { - for(Key key: *extraReelimKeys) { + if (extraReelimKeys) { + for (Key key : *extraReelimKeys) { markedKeys.insert(key); } } // Observed keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: markedKeys) { + if (params_.enableDetailedResults) { + for (Key key : markedKeys) { result.detail->variableStatus[key].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Key value) instead of the iterator constructor. - FastVector observedKeys; observedKeys.reserve(markedKeys.size()); - for(Key index: markedKeys) { - if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused - observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them + FastVector observedKeys; + observedKeys.reserve(markedKeys.size()); + for (Key index : markedKeys) { + if (unusedIndices.find(index) == + unusedIndices.end()) // Only add if not unused + observedKeys.push_back( + index); // Make a copy of these, as we'll soon add to them } gttoc(gather_involved_keys); - // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + // Check relinearization if we're at the nth step, or we are using a looser + // loop relin threshold KeySet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); - // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - if(params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial(roots_, delta_, params_.relinearizeThreshold); + // 4. Mark keys in \Delta above threshold \beta: + // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + if (params_.enablePartialRelinearizationCheck) + relinKeys = Impl::CheckRelinearizationPartial( + roots_, delta_, params_.relinearizeThreshold); else - relinKeys = Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging + relinKeys = + Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); + if (kDisableReordering) + relinKeys = Impl::CheckRelinearizationFull( + delta_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed - for(Key key: fixedVariables_) { + for (Key key : fixedVariables_) { relinKeys.erase(key); } - if(noRelinKeys) { - for(Key key: *noRelinKeys) { + if (noRelinKeys) { + for (Key key : *noRelinKeys) { relinKeys.erase(key); } } // Above relin threshold keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: relinKeys) { + if (params_.enableDetailedResults) { + for (Key key : relinKeys) { result.detail->variableStatus[key].isAboveRelinThreshold = true; - result.detail->variableStatus[key].isRelinearized = true; } } + result.detail->variableStatus[key].isRelinearized = true; + } + } // Add the variables being relinearized to the marked keys KeySet markedRelinMask; - for(const Key key: relinKeys) - markedRelinMask.insert(key); + for (const Key key : relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); gttoc(gather_relinearize_keys); gttic(fluid_find_all); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. + // 5. Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. if (!relinKeys.empty()) { - for(const sharedClique& root: roots_) + for (const sharedClique& root : roots_) // add other cliques that have the marked ones in the separator - Impl::FindAll(root, markedKeys, markedRelinMask); + root->findAll(markedRelinMask, &markedKeys); // Relin involved keys for detailed results - if(params_.enableDetailedResults) { + if (params_.enableDetailedResults) { KeySet involvedRelinKeys; - for(const sharedClique& root: roots_) - Impl::FindAll(root, involvedRelinKeys, markedRelinMask); - for(Key key: involvedRelinKeys) { - if(!result.detail->variableStatus[key].isAboveRelinThreshold) { + for (const sharedClique& root : roots_) + root->findAll(markedRelinMask, &involvedRelinKeys); + for (Key key : involvedRelinKeys) { + if (!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; - result.detail->variableStatus[key].isRelinearized = true; } } + result.detail->variableStatus[key].isRelinearized = true; + } + } } } gttoc(fluid_find_all); gttic(expmap); - // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, markedRelinMask, delta_); + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. + if (!relinKeys.empty()) expmapMasked(markedRelinMask); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -706,17 +754,15 @@ ISAM2Result ISAM2::update( gttic(linearize_new); // 7. Linearize new factors - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { gttic(linearize); - GaussianFactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_); - if(params_.findUnusedFactorSlots) - { + auto linearFactors = newFactors.linearize(theta_); + if (params_.findUnusedFactorSlots) { linearFactors_.resize(nonlinearFactors_.size()); - for(size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) - linearFactors_[result.newFactorsIndices[newFactorI]] = (*linearFactors)[newFactorI]; - } - else - { + for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) + linearFactors_[result.newFactorsIndices[newFactorI]] = + (*linearFactors)[newFactorI]; + } else { linearFactors_.push_back(*linearFactors); } assert(nonlinearFactors_.size() == linearFactors_.size()); @@ -726,7 +772,7 @@ ISAM2Result ISAM2::update( gttic(augment_VI); // Augment the variable index with the new factors - if(params_.findUnusedFactorSlots) + if (params_.findUnusedFactorSlots) variableIndex_.augment(newFactors, result.newFactorsIndices); else variableIndex_.augment(newFactors); @@ -734,26 +780,26 @@ ISAM2Result ISAM2::update( gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr replacedKeys; - if(!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); + boost::shared_ptr replacedKeys; + if (!markedKeys.empty() || !observedKeys.empty()) + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, + unusedIndices, constrainedKeys, &result); // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) + if (replacedKeys) deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); gttoc(recalculate); // Update data structures to remove unused keys - if(!unusedKeys.empty()) { + if (!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, Base::nodes_, fixedVariables_); + removeVariables(unusedKeys); gttoc(remove_variables); } result.cliques = this->nodes().size(); gttic(evaluate_error_after); - if(params_.evaluateNonlinearError) + if (params_.evaluateNonlinearError) result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_after); @@ -761,36 +807,60 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -void ISAM2::marginalizeLeaves(const FastList& leafKeysList, - boost::optional marginalFactorsIndices, - boost::optional deletedFactorsIndices) -{ +void ISAM2::marginalizeLeaves( + const FastList& leafKeysList, + boost::optional marginalFactorsIndices, + boost::optional deletedFactorsIndices) { // Convert to ordered set KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. -// multimap marginalFactors; + // multimap marginalFactors; map > marginalFactors; - // Keep track of factors that get summarized by removing cliques - KeySet factorIndicesToRemove; - // Keep track of variables removed in subtrees KeySet leafKeysRemoved; + // Keep track of factors that get summarized by removing cliques + KeySet factorIndicesToRemove; + + // Remove the subtree and throw away the cliques + auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) { + const Cliques removedCliques = this->removeSubtree(subtreeRoot); + for (const sharedClique& removedClique : removedCliques) { + auto cg = removedClique->conditional(); + marginalFactors.erase(cg->front()); + leafKeysRemoved.insert(cg->beginFrontals(), cg->endFrontals()); + for (Key frontal : cg->frontals()) { + // Add to factors to remove + const auto& involved = variableIndex_[frontal]; + factorIndicesToRemove.insert(involved.begin(), involved.end()); +#if !defined(NDEBUG) + // Check for non-leaf keys + if (!leafKeys.exists(frontal)) + throw std::runtime_error( + "Requesting to marginalize variables that are not leaves, " + "the ISAM2 object is now in an inconsistent state so should " + "no longer be used."); +#endif + } + } + return removedCliques; + }; + // Remove each variable and its subtrees - for(Key j: leafKeys) { - if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree + for (Key j : leafKeys) { + if (!leafKeysRemoved.exists(j)) { // If the index was not already removed + // by removing another subtree // Traverse up the tree to find the root of the marginalized subtree sharedClique clique = nodes_[j]; - while(!clique->parent_._empty()) - { - // Check if parent contains a marginalized leaf variable. Only need to check the first - // variable because it is the closest to the leaves. + while (!clique->parent_._empty()) { + // Check if parent contains a marginalized leaf variable. Only need to + // check the first variable because it is the closest to the leaves. sharedClique parent = clique->parent(); - if(leafKeys.exists(parent->conditional()->front())) + if (leafKeys.exists(parent->conditional()->front())) clique = parent; else break; @@ -798,39 +868,28 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // See if we should remove the whole clique bool marginalizeEntireClique = true; - for(Key frontal: clique->conditional()->frontals()) { - if(!leafKeys.exists(frontal)) { + for (Key frontal : clique->conditional()->frontals()) { + if (!leafKeys.exists(frontal)) { marginalizeEntireClique = false; - break; } } + break; + } + } // Remove either the whole clique or part of it - if(marginalizeEntireClique) { - // Remove the whole clique and its subtree, and keep the marginal factor. - GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); + if (marginalizeEntireClique) { + // Remove the whole clique and its subtree, and keep the marginal + // factor. + auto marginalFactor = clique->cachedFactor(); // We do not need the marginal factors associated with this clique // because their information is already incorporated in the new // marginal factor. So, now associate this marginal factor with the // parent of this clique. - marginalFactors[clique->parent()->conditional()->front()].push_back(marginalFactor); + marginalFactors[clique->parent()->conditional()->front()].push_back( + marginalFactor); // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. - const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques - for(const sharedClique& removedClique: removedCliques) { - marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { - // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); - - // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); - } - } - } - else { + trackingRemoveSubtree(clique); + } else { // Reeliminate the current clique and the marginals from its children, // then keep only the marginal on the non-marginalized variables. We // get the childrens' marginals from any existing children, plus @@ -841,181 +900,189 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, GaussianFactorGraph graph; KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; - for(const sharedClique& child: clique->children) { + for (const sharedClique& child : clique->children) { // Remove subtree if child depends on any marginalized keys - for(Key parent: child->conditional()->parents()) { - if(leafKeys.exists(parent)) { + for (Key parent : child->conditional()->parents()) { + if (leafKeys.exists(parent)) { subtreesToRemove.push_back(child); - graph.push_back(child->cachedFactor()); // Add child marginal + graph.push_back(child->cachedFactor()); // Add child marginal break; } } } Cliques childrenRemoved; - for(const sharedClique& childToRemove: subtreesToRemove) { - const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques - childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); - for(const sharedClique& removedClique: removedCliques) { - marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { - // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + for (const sharedClique& subtree : subtreesToRemove) { + const Cliques removed = trackingRemoveSubtree(subtree); + childrenRemoved.insert(childrenRemoved.end(), removed.begin(), + removed.end()); + } - // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + // Add the factors that are pulled into the current clique by the + // marginalized variables. These are the factors that involve + // *marginalized* frontal variables in this clique but do not involve + // frontal variables of any of its children. + // TODO(dellaert): reuse cached linear factors + KeySet factorsFromMarginalizedInClique_step1; + for (Key frontal : clique->conditional()->frontals()) { + if (leafKeys.exists(frontal)) + factorsFromMarginalizedInClique_step1.insert( + variableIndex_[frontal].begin(), variableIndex_[frontal].end()); + } + // Remove any factors in subtrees that we're removing at this step + for (const sharedClique& removedChild : childrenRemoved) { + for (Key indexInClique : removedChild->conditional()->frontals()) { + for (Key factorInvolving : variableIndex_[indexInClique]) { + factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } - - // Add the factors that are pulled into the current clique by the marginalized variables. - // These are the factors that involve *marginalized* frontal variables in this clique - // but do not involve frontal variables of any of its children. - // TODO: reuse cached linear factors - KeySet factorsFromMarginalizedInClique_step1; - for(Key frontal: clique->conditional()->frontals()) { - if(leafKeys.exists(frontal)) - factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } - // Remove any factors in subtrees that we're removing at this step - for(const sharedClique& removedChild: childrenRemoved) { - for(Key indexInClique: removedChild->conditional()->frontals()) { - for(Key factorInvolving: variableIndex_[indexInClique]) { - factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } // Create factor graph from factor indices - for(size_t i: factorsFromMarginalizedInClique_step1) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } + for (size_t i : factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[i]->linearize(theta_)); + } - // Reeliminate the linear graph to get the marginal and discard the conditional - const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + // Reeliminate the linear graph to get the marginal and discard the + // conditional + auto cg = clique->conditional(); + const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals()); FastVector cliqueFrontalsToEliminate; - std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), - std::back_inserter(cliqueFrontalsToEliminate)); - pair eliminationResult1 = - params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminate)); + std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), + leafKeys.begin(), leafKeys.end(), + std::back_inserter(cliqueFrontalsToEliminate)); + auto eliminationResult1 = params_.getEliminationFunction()( + graph, Ordering(cliqueFrontalsToEliminate)); // Add the resulting marginal - if(eliminationResult1.second) - marginalFactors[clique->conditional()->front()].push_back(eliminationResult1.second); + if (eliminationResult1.second) + marginalFactors[cg->front()].push_back(eliminationResult1.second); // Split the current clique // Find the position of the last leaf key in this clique DenseIndex nToRemove = 0; - while(leafKeys.exists(clique->conditional()->keys()[nToRemove])) - ++ nToRemove; + while (leafKeys.exists(cg->keys()[nToRemove])) ++nToRemove; // Make the clique's matrix appear as a subset - const DenseIndex dimToRemove = clique->conditional()->matrixObject().offset(nToRemove); - clique->conditional()->matrixObject().firstBlock() = nToRemove; - clique->conditional()->matrixObject().rowStart() = dimToRemove; + const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove); + cg->matrixObject().firstBlock() = nToRemove; + cg->matrixObject().rowStart() = dimToRemove; // Change the keys in the clique - FastVector originalKeys; originalKeys.swap(clique->conditional()->keys()); - clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); - clique->conditional()->nrFrontals() -= nToRemove; + FastVector originalKeys; + originalKeys.swap(cg->keys()); + cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); + cg->nrFrontals() -= nToRemove; - // Add to factors to remove factors involved in frontals of current clique - for(Key frontal: cliqueFrontalsToEliminate) - { - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + // Add to factorIndicesToRemove any factors involved in frontals of + // current clique + for (Key frontal : cliqueFrontalsToEliminate) { + const auto& involved = variableIndex_[frontal]; + factorIndicesToRemove.insert(involved.begin(), involved.end()); } // Add removed keys - leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); + leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), + cliqueFrontalsToEliminate.end()); } } } - // At this point we have updated the BayesTree, now update the remaining iSAM2 data structures + // At this point we have updated the BayesTree, now update the remaining iSAM2 + // data structures // Gather factors to add - the new marginal factors GaussianFactorGraph factorsToAdd; - typedef pair > Key_Factors; - for(const Key_Factors& key_factors: marginalFactors) { - for(const GaussianFactor::shared_ptr& factor: key_factors.second) { - if(factor) { + for (const auto& key_factors : marginalFactors) { + for (const auto& factor : key_factors.second) { + if (factor) { factorsToAdd.push_back(factor); - if(marginalFactorsIndices) + if (marginalFactorsIndices) marginalFactorsIndices->push_back(nonlinearFactors_.size()); - nonlinearFactors_.push_back(boost::make_shared( - factor)); - if(params_.cacheLinearizedFactors) - linearFactors_.push_back(factor); - for(Key factorKey: *factor) { - fixedVariables_.insert(factorKey); } + nonlinearFactors_.push_back( + boost::make_shared(factor)); + if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor); + for (Key factorKey : *factor) { + fixedVariables_.insert(factorKey); + } } } } - variableIndex_.augment(factorsToAdd); // Augment the variable index + variableIndex_.augment(factorsToAdd); // Augment the variable index - // Remove the factors to remove that have been summarized in the newly-added marginal factors + // Remove the factors to remove that have been summarized in the newly-added + // marginal factors NonlinearFactorGraph removedFactors; - for(size_t i: factorIndicesToRemove) { + for (size_t i : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); - if(params_.cacheLinearizedFactors) - linearFactors_.remove(i); + if (params_.cacheLinearizedFactors) linearFactors_.remove(i); } - variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); + variableIndex_.remove(factorIndicesToRemove.begin(), + factorIndicesToRemove.end(), removedFactors); - if(deletedFactorsIndices) - deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); + if (deletedFactorsIndices) + deletedFactorsIndices->assign(factorIndicesToRemove.begin(), + factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, nodes_, fixedVariables_); + removeVariables(KeySet(leafKeys.begin(), leafKeys.end())); } /* ************************************************************************* */ -void ISAM2::updateDelta(bool forceFullSolve) const -{ +void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); - if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { // If using Gauss-Newton, update with wildfireThreshold const ISAM2GaussNewtonParams& gaussNewtonParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( - roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); + roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + } else if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; // Do one Dogleg iteration gttic(Dogleg_Iterate); // Compute Newton's method step gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); + lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_); gttoc(Wildfire_update); // Compute steepest descent step - const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient - Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd - const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point + const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient + Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, + &RgProd_); // Update RgProd + const VectorValues dx_u = Impl::ComputeGradientSearch( + gradAtZero, RgProd_); // Compute gradient search point - // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ + // Clear replaced keys mask because now we've updated deltaNewton_ and + // RgProd_ deltaReplacedMask_.clear(); // Compute dogleg point - DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, - theta_, nonlinearFactors_.error(theta_), doglegParams.verbose)); + DoglegOptimizerImpl::IterationResult doglegResult( + DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, + *this, nonlinearFactors_, theta_, nonlinearFactors_.error(theta_), + doglegParams.verbose)); gttoc(Dogleg_Iterate); gttic(Copy_dx_d); // Update Delta and linear step doglegDelta_ = doglegResult.delta; - delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + delta_ = + doglegResult + .dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); } } @@ -1037,60 +1104,61 @@ const Value& ISAM2::calculateEstimate(Key key) const { /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - updateDelta(true); // Force full solve when updating delta_ + updateDelta(true); // Force full solve when updating delta_ return theta_.retract(delta_); } /* ************************************************************************* */ Matrix ISAM2::marginalCovariance(Key key) const { - return marginalFactor(key, params_.getEliminationFunction())->information().inverse(); + return marginalFactor(key, params_.getEliminationFunction()) + ->information() + .inverse(); } /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { - if(!deltaReplacedMask_.empty()) - updateDelta(); + if (!deltaReplacedMask_.empty()) updateDelta(); return delta_; } /* ************************************************************************* */ -double ISAM2::error(const VectorValues& x) const -{ +double ISAM2::error(const VectorValues& x) const { return GaussianFactorGraph(*this).error(x); } /* ************************************************************************* */ -static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { +static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, + VectorValues* g) { // Loop through variables in each clique, adding contributions DenseIndex variablePosition = 0; - for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { + for (GaussianConditional::const_iterator jit = root->conditional()->begin(); + jit != root->conditional()->end(); ++jit) { const DenseIndex dim = root->conditional()->getDim(jit); - pair pos_ins = - g.tryInsert(*jit, root->gradientContribution().segment(variablePosition, dim)); - if(!pos_ins.second) - pos_ins.first->second += root->gradientContribution().segment(variablePosition, dim); + pair pos_ins = g->tryInsert( + *jit, root->gradientContribution().segment(variablePosition, dim)); + if (!pos_ins.second) + pos_ins.first->second += + root->gradientContribution().segment(variablePosition, dim); variablePosition += dim; } // Recursively add contributions from children typedef boost::shared_ptr sharedClique; - for(const sharedClique& child: root->children) { + for (const sharedClique& child : root->children) { gradientAtZeroTreeAdder(child, g); } } /* ************************************************************************* */ -VectorValues ISAM2::gradientAtZero() const -{ +VectorValues ISAM2::gradientAtZero() const { // Create result VectorValues g; // Sum up contributions for each clique - for(const ISAM2::sharedClique& root: this->roots()) - gradientAtZeroTreeAdder(root, g); + for (const ISAM2::sharedClique& root : this->roots()) + gradientAtZeroTreeAdder(root, &g); return g; } -} -/// namespace gtsam +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 114c04018..5d448d786 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -11,438 +11,47 @@ /** * @file ISAM2.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ // \callgraph #pragma once +#include +#include +#include #include -#include #include -#include +#include namespace gtsam { /** * @addtogroup ISAM2 - * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or - * ISAM2DoglegParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2GaussNewtonParams { - double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001) - - /** Specify parameters as constructor arguments */ - ISAM2GaussNewtonParams( - double _wildfireThreshold = 0.001 ///< see ISAM2GaussNewtonParams public variables, ISAM2GaussNewtonParams::wildfireThreshold - ) : wildfireThreshold(_wildfireThreshold) {} - - void print(const std::string str = "") const { - std::cout << str << "type: ISAM2GaussNewtonParams\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout.flush(); - } - - double getWildfireThreshold() const { return wildfireThreshold; } - void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; } -}; - -/** - * @addtogroup ISAM2 - * Parameters for ISAM2 using Dogleg optimization. Either this class or - * ISAM2GaussNewtonParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2DoglegParams { - double initialDelta; ///< The initial trust region radius for Dogleg - double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 1e-5) - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode - bool verbose; ///< Whether Dogleg prints iteration and convergence information - - /** Specify parameters as constructor arguments */ - ISAM2DoglegParams( - double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta - double _wildfireThreshold = 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold - DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode - bool _verbose = false ///< see ISAM2DoglegParams::verbose - ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold), - adaptationMode(_adaptationMode), verbose(_verbose) {} - - void print(const std::string str = "") const { - std::cout << str << "type: ISAM2DoglegParams\n"; - std::cout << str << "initialDelta: " << initialDelta << "\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout << str << "adaptationMode: " << adaptationModeTranslator(adaptationMode) << "\n"; - std::cout.flush(); - } - - double getInitialDelta() const { return initialDelta; } - double getWildfireThreshold() const { return wildfireThreshold; } - std::string getAdaptationMode() const { return adaptationModeTranslator(adaptationMode); }; - bool isVerbose() const { return verbose; }; - - void setInitialDelta(double initialDelta) { this->initialDelta = initialDelta; } - void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; } - void setAdaptationMode(const std::string& adaptationMode) { this->adaptationMode = adaptationModeTranslator(adaptationMode); } - void setVerbose(bool verbose) { this->verbose = verbose; }; - - std::string adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const; - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(const std::string& adaptationMode) const; -}; - -/** - * @addtogroup ISAM2 - * Parameters for the ISAM2 algorithm. Default parameter values are listed below. - */ -typedef FastMap ISAM2ThresholdMap; -typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; -struct GTSAM_EXPORT ISAM2Params { - typedef boost::variant OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams - typedef boost::variant > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds - - /** Optimization parameters, this both selects the nonlinear optimization - * method and specifies its parameters, either ISAM2GaussNewtonParams or - * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used - * with the specified parameters, and in the latter Powell's dog-leg - * algorithm will be used with the specified parameters. - */ - OptimizationParams optimizationParams; - - /** Only relinearize variables whose linear delta magnitude is greater than - * this threshold (default: 0.1). If this is a FastMap instead - * of a double, then the threshold is specified for each dimension of each - * variable type. This parameter then maps from a character indicating the - * variable type to a Vector of thresholds for each dimension of that - * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, - * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate - * entries would be added with: - * \code - FastMap thresholds; - thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold - params.relinearizeThreshold = thresholds; - \endcode - */ - RelinearizationThreshold relinearizeThreshold; - - int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10) - - bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true) - - bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update() - - enum Factorization { CHOLESKY, QR }; - /** Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY). - * Cholesky is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when - * uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is - * slower but more numerically stable in poorly-conditioned problems. We suggest using the default of Cholesky - * unless gtsam sometimes throws IndefiniteLinearSystemException when your problem's Hessian is actually positive - * definite. For positive definite problems, numerical error accumulation can cause the problem to become - * numerically negative or indefinite as solving proceeds, especially when using Cholesky. - */ - Factorization factorization; - - /** Whether to cache linear factors (default: true). - * This can improve performance if linearization is expensive, but can hurt - * performance if linearization is very cleap due to computation to look up - * additional keys. - */ - bool cacheLinearizedFactors; - - KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) - - bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false) - - /** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false). - * This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate - * significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance - * is desired over correctness. Use with caution. - */ - bool enablePartialRelinearizationCheck; - - /// When you will be removing many factors, e.g. when using ISAM2 as a fixed-lag smoother, enable this option to - /// add factors in the first available factor slots, to avoid accumulating NULL factor slots, at the cost of - /// having to search for slots every time a factor is added. - bool findUnusedFactorSlots; - - /** Specify parameters as constructor arguments */ - ISAM2Params( - OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams - RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold - int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip - bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization - bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError - Factorization _factorization = ISAM2Params::CHOLESKY, ///< see ISAM2Params::factorization - bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors - const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), - relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), - evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), - cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), enablePartialRelinearizationCheck(false), - findUnusedFactorSlots(false) {} - - /// print iSAM2 parameters - void print(const std::string& str = "") const { - std::cout << str << "\n"; - if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - boost::get(optimizationParams).print("optimizationParams: "); - else if(optimizationParams.type() == typeid(ISAM2DoglegParams)) - boost::get(optimizationParams).print("optimizationParams: "); - else - std::cout << "optimizationParams: " << "{unknown type}" << "\n"; - if(relinearizeThreshold.type() == typeid(double)) - std::cout << "relinearizeThreshold: " << boost::get(relinearizeThreshold) << "\n"; - else - { - std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; - for(const ISAM2ThresholdMapValue& value: boost::get(relinearizeThreshold)) { - std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; - } - } - std::cout << "relinearizeSkip: " << relinearizeSkip << "\n"; - std::cout << "enableRelinearization: " << enableRelinearization << "\n"; - std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n"; - std::cout << "factorization: " << factorizationTranslator(factorization) << "\n"; - std::cout << "cacheLinearizedFactors: " << cacheLinearizedFactors << "\n"; - std::cout << "enableDetailedResults: " << enableDetailedResults << "\n"; - std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n"; - std::cout << "findUnusedFactorSlots: " << findUnusedFactorSlots << "\n"; - std::cout.flush(); - } - - /// @name Getters and Setters for all properties - /// @{ - - OptimizationParams getOptimizationParams() const { return this->optimizationParams; } - RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } - int getRelinearizeSkip() const { return relinearizeSkip; } - bool isEnableRelinearization() const { return enableRelinearization; } - bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } - std::string getFactorization() const { return factorizationTranslator(factorization); } - bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } - KeyFormatter getKeyFormatter() const { return keyFormatter; } - bool isEnableDetailedResults() const { return enableDetailedResults; } - bool isEnablePartialRelinearizationCheck() const { return enablePartialRelinearizationCheck; } - - void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; } - void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; } - void setRelinearizeSkip(int relinearizeSkip) { this->relinearizeSkip = relinearizeSkip; } - void setEnableRelinearization(bool enableRelinearization) { this->enableRelinearization = enableRelinearization; } - void setEvaluateNonlinearError(bool evaluateNonlinearError) { this->evaluateNonlinearError = evaluateNonlinearError; } - void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); } - void setCacheLinearizedFactors(bool cacheLinearizedFactors) { this->cacheLinearizedFactors = cacheLinearizedFactors; } - void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; } - void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; } - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } - - GaussianFactorGraph::Eliminate getEliminationFunction() const { - return factorization == CHOLESKY - ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky - : (GaussianFactorGraph::Eliminate)EliminateQR; - } - - /// @} - - /// @name Some utilities - /// @{ - - static Factorization factorizationTranslator(const std::string& str); - static std::string factorizationTranslator(const Factorization& value); - - /// @} -}; - -typedef FastVector FactorIndices; - -/** - * @addtogroup ISAM2 - * This struct is returned from ISAM2::update() and contains information about - * the update that is useful for determining whether the solution is - * converging, and about how much work was required for the update. See member - * variables for details and information about each entry. - */ -struct GTSAM_EXPORT ISAM2Result { - /** The nonlinear error of all of the factors, \a including new factors and - * variables added during the current call to ISAM2::update(). This error is - * calculated using the following variable values: - * \li Pre-existing variables will be evaluated by combining their - * linearization point before this call to update, with their partial linear - * delta, as computed by ISAM2::calculateEstimate(). - * \li New variables will be evaluated at their initialization points passed - * into the current call to update. - * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError - * is set to \c true, because there is some cost to this computation. - */ - boost::optional errorBefore; - - /** The nonlinear error of all of the factors computed after the current - * update, meaning that variables above the relinearization threshold - * (ISAM2Params::relinearizeThreshold) have been relinearized and new - * variables have undergone one linear update. Variable values are - * again computed by combining their linearization points with their - * partial linear deltas, by ISAM2::calculateEstimate(). - * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError - * is set to \c true, because there is some cost to this computation. - */ - boost::optional errorAfter; - - /** The number of variables that were relinearized because their linear - * deltas exceeded the reslinearization threshold - * (ISAM2Params::relinearizeThreshold), combined with any additional - * variables that had to be relinearized because they were involved in - * the same factor as a variable above the relinearization threshold. - * On steps where no relinearization is considered - * (see ISAM2Params::relinearizeSkip), this count will be zero. - */ - size_t variablesRelinearized; - - /** The number of variables that were reeliminated as parts of the Bayes' - * Tree were recalculated, due to new factors. When loop closures occur, - * this count will be large as the new loop-closing factors will tend to - * involve variables far away from the root, and everything up to the root - * will be reeliminated. - */ - size_t variablesReeliminated; - - /** The number of factors that were included in reelimination of the Bayes' tree. */ - size_t factorsRecalculated; - - /** The number of cliques in the Bayes' Tree */ - size_t cliques; - - /** The indices of the newly-added factors, in 1-to-1 correspondence with the - * factors passed as \c newFactors to ISAM2::update(). These indices may be - * used later to refer to the factors in order to remove them. - */ - FactorIndices newFactorsIndices; - - /** A struct holding detailed results, which must be enabled with - * ISAM2Params::enableDetailedResults. - */ - struct DetailedResults { - /** The status of a single variable, this struct is stored in - * DetailedResults::variableStatus */ - struct VariableStatus { - /** Whether the variable was just reeliminated, due to being relinearized, - * observed, new, or on the path up to the root clique from another - * reeliminated variable. */ - bool isReeliminated; - bool isAboveRelinThreshold; ///< Whether the variable was just relinearized due to being above the relinearization threshold - bool isRelinearizeInvolved; ///< Whether the variable was below the relinearization threshold but was relinearized by being involved in a factor with a variable above the relinearization threshold - bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement. - bool isObserved; ///< Whether the variable was just involved in new factors - bool isNew; ///< Whether the variable itself was just added - bool inRootClique; ///< Whether the variable is in the root clique - VariableStatus(): isReeliminated(false), isAboveRelinThreshold(false), isRelinearizeInvolved(false), - isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {} - }; - - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; - }; - - /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See - * Detail for information about the results data stored here. */ - boost::optional detail; - - - void print(const std::string str = "") const { - std::cout << str << " Reelimintated: " << variablesReeliminated << " Relinearized: " << variablesRelinearized << " Cliques: " << cliques << std::endl; - } - - /** Getters and Setters */ - size_t getVariablesRelinearized() const { return variablesRelinearized; }; - size_t getVariablesReeliminated() const { return variablesReeliminated; }; - size_t getCliques() const { return cliques; }; -}; - -/** - * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution - * TODO: more documentation - */ -class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase -{ -public: - typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - typedef GaussianConditional ConditionalType; - typedef ConditionalType::shared_ptr sharedConditional; - - Base::FactorType::shared_ptr cachedFactor_; - Vector gradientContribution_; - FastMap solnPointers_; - - /// Default constructor - ISAM2Clique() : Base() {} - - /// Copy constructor, does *not* copy solution pointers as these are invalid in different trees. - ISAM2Clique(const ISAM2Clique& other) : - Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {} - - /// Assignment operator, does *not* copy solution pointers as these are invalid in different trees. - ISAM2Clique& operator=(const ISAM2Clique& other) - { - Base::operator=(other); - cachedFactor_ = other.cachedFactor_; - gradientContribution_ = other.gradientContribution_; - return *this; - } - - /// Overridden to also store the remaining factor and gradient contribution - void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult); - - /** Access the cached factor */ - Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - - /** Access the gradient contribution */ - const Vector& gradientContribution() const { return gradientContribution_; } - - bool equals(const This& other, double tol=1e-9) const; - - /** print this node */ - void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(cachedFactor_); - ar & BOOST_SERIALIZATION_NVP(gradientContribution_); - } -}; // \struct ISAM2Clique - -/** - * @addtogroup ISAM2 - * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. + * Implementation of the full ISAM2 algorithm for incremental nonlinear + * optimization. * - * The typical cycle of using this class to create an instance by providing ISAM2Params - * to the constructor, then add measurements and variables as they arrive using the update() - * method. At any time, calculateEstimate() may be called to obtain the current - * estimate of all variables. + * The typical cycle of using this class to create an instance by providing + * ISAM2Params to the constructor, then add measurements and variables as they + * arrive using the update() method. At any time, calculateEstimate() may be + * called to obtain the current estimate of all variables. * */ -class GTSAM_EXPORT ISAM2: public BayesTree { - -protected: - +class GTSAM_EXPORT ISAM2 : public BayesTree { + protected: /** The current linearization point */ Values theta_; - /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ + /** VariableIndex lets us look up factors by involved variable and keeps track + * of dimensions */ VariableIndex variableIndex_; - /** The linear delta from the last linear solution, an update to the estimate in theta + /** The linear delta from the last linear solution, an update to the estimate + * in theta * * This is \c mutable because it is a "cached" variable - it is not updated * until either requested with getDelta() or calculateEstimate(), or needed @@ -450,8 +59,10 @@ protected: */ mutable VectorValues delta_; - mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update - mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally + mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores + // the Gauss-Newton update + mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and + // is updated incrementally /** A cumulative mask for the variables that were replaced and have not yet * been updated in the linear solution delta_, this is only used internally, @@ -461,9 +72,11 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable KeySet + deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way - /** All original nonlinear factors are stored here to use during relinearization */ + /** All original nonlinear factors are stored here to use during + * relinearization */ NonlinearFactorGraph nonlinearFactors_; /** The current linear factors, which are only updated as needed */ @@ -479,20 +92,21 @@ protected: * variables and thus cannot have their linearization points changed. */ KeySet fixedVariables_; - int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization + int update_count_; ///< Counter incremented every update(), used to determine + ///< periodic relinearization -public: - - typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class - typedef Base::Clique Clique; ///< A clique - typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + public: + typedef ISAM2 This; ///< This class + typedef BayesTree Base; ///< The BayesTree base class + typedef Base::Clique Clique; ///< A clique + typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique + typedef Base::Cliques Cliques; ///< List of Clique typedef from base class /** Create an empty ISAM2 instance */ - ISAM2(const ISAM2Params& params); + explicit ISAM2(const ISAM2Params& params); - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ + /** Create an empty ISAM2 instance using the default set of parameters (see + * ISAM2Params) */ ISAM2(); /** default virtual destructor */ @@ -513,26 +127,31 @@ public: * thresholds. * * @param newFactors The new factors to be added to the system - * @param newTheta Initialization points for new variables to be added to the system. - * You must include here all new variables occuring in newFactors (which were not already - * in the system). There must not be any variables here that do not occur in newFactors, - * and additionally, variables that were already in the system must not be included here. + * @param newTheta Initialization points for new variables to be added to the + * system. You must include here all new variables occuring in newFactors + * (which were not already in the system). There must not be any variables + * here that do not occur in newFactors, and additionally, variables that were + * already in the system must not be included here. * @param removeFactorIndices Indices of factors to remove from system - * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently - * large (Params::relinearizeThreshold), regardless of the relinearization interval - * (Params::relinearizeSkip). - * @param constrainedKeys is an optional map of keys to group labels, such that a variable can - * be constrained to a particular grouping in the BayesTree - * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization - * point, regardless of the size of the linear delta - * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless - * of the size of the linear delta. This allows the provided keys to be reordered. + * @param force_relinearize Relinearize any variables whose delta magnitude is + * sufficiently large (Params::relinearizeThreshold), regardless of the + * relinearization interval (Params::relinearizeSkip). + * @param constrainedKeys is an optional map of keys to group labels, such + * that a variable can be constrained to a particular grouping in the + * BayesTree + * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will + * hold at a constant linearization point, regardless of the size of the + * linear delta + * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will + * re-eliminate, regardless of the size of the linear delta. This allows the + * provided keys to be reordered. * @return An ISAM2Result struct containing information about the update */ - virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + virtual ISAM2Result update( + const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FactorIndices& removeFactorIndices = FactorIndices(), - const boost::optional >& constrainedKeys = boost::none, + const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, bool force_relinearize = false); @@ -542,48 +161,54 @@ public: * requested to be marginalized. Marginalization leaves a linear * approximation of the marginal in the system, and the linearization points * of any variables involved in this linear marginal become fixed. The set - * fixed variables will include any key involved with the marginalized variables - * in the original factors, and possibly additional ones due to fill-in. + * fixed variables will include any key involved with the marginalized + * variables in the original factors, and possibly additional ones due to + * fill-in. * - * If provided, 'marginalFactorsIndices' will be augmented with the factor graph - * indices of the marginal factors added during the 'marginalizeLeaves' call + * If provided, 'marginalFactorsIndices' will be augmented with the factor + * graph indices of the marginal factors added during the 'marginalizeLeaves' + * call * - * If provided, 'deletedFactorsIndices' will be augmented with the factor graph - * indices of any factor that was removed during the 'marginalizeLeaves' call + * If provided, 'deletedFactorsIndices' will be augmented with the factor + * graph indices of any factor that was removed during the 'marginalizeLeaves' + * call */ - void marginalizeLeaves(const FastList& leafKeys, - boost::optional marginalFactorsIndices = boost::none, - boost::optional deletedFactorsIndices = boost::none); + void marginalizeLeaves( + const FastList& leafKeys, + boost::optional marginalFactorsIndices = boost::none, + boost::optional deletedFactorsIndices = boost::none); /// Access the current linearization point - const Values& getLinearizationPoint() const { - return theta_; - } + const Values& getLinearizationPoint() const { return theta_; } /// Check whether variable with given key exists in linearization point - bool valueExists(Key key) const { - return theta_.exists(key); - } + bool valueExists(Key key) const { return theta_.exists(key); } - /** Compute an estimate from the incomplete linear delta computed during the last update. - * This delta is incomplete because it was not updated below wildfire_threshold. If only - * a single variable is needed, it is faster to call calculateEstimate(const KEY&). + /** Compute an estimate from the incomplete linear delta computed during the + * last update. This delta is incomplete because it was not updated below + * wildfire_threshold. If only a single variable is needed, it is faster to + * call calculateEstimate(const KEY&). */ Values calculateEstimate() const; - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. * @param key * @return */ - template - VALUE calculateEstimate(Key key) const; + template + VALUE calculateEstimate(Key key) const { + const Vector& delta = getDelta()[key]; + return traits::Retract(theta_.at(key), delta); + } - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. This is a non-templated version - * that returns a Value base class for use with the MATLAB wrapper. + + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. + * This is a non-templated version that returns a Value base class for use + * with the MATLAB wrapper. * @param key * @return */ @@ -598,7 +223,8 @@ public: /** Internal implementation functions */ struct Impl; - /** Compute an estimate using a complete delta computed by a full back-substitution. + /** Compute an estimate using a complete delta computed by a full + * back-substitution. */ Values calculateBestEstimate() const; @@ -609,7 +235,9 @@ public: double error(const VectorValues& x) const; /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { + return nonlinearFactors_; + } /** Access the nonlinear variable index */ const VariableIndex& getVariableIndex() const { return variableIndex_; } @@ -628,56 +256,60 @@ public: /** prints out clique statistics */ void printStats() const { getCliqueData().getStats().print(); } - - /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d - * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also - * gradient(const GaussianBayesNet&, const VectorValues&). + + /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert + * \Sigma^{-1} R x - d \right\Vert^2 \f$, centered around zero. The gradient + * about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, + * const VectorValues&). * * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - + /// @} -protected: + protected: + /** + * Add new variables to the ISAM2 system. + * @param newTheta Initial values for new variables + * @param theta Current solution to be augmented with new initialization + * @param delta Current linear delta to be augmented with zeros + * @param deltaNewton + * @param RgProd + * @param keyFormatter Formatter for printing nonlinear keys during debugging + */ + void addVariables(const Values& newTheta); + + /** + * Remove variables from the ISAM2 system. + */ + void removeVariables(const KeySet& unusedKeys); + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c mask. Values are expmapped in-place. + * \param mask Mask on linear indices, only \c true entries are expmapped + */ + void expmapMasked(const KeySet& mask); FastSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; - GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const; + GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); + + virtual boost::shared_ptr recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const std::vector& observedKeys, const KeySet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result* result); - virtual boost::shared_ptr recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; - -}; // ISAM2 +}; // ISAM2 /// traits -template<> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; -/// Optimize the BayesTree, starting from the root. -/// @param replaced Needs to contain -/// all variables that are contained in the top of the Bayes tree that has been -/// redone. -/// @param delta The current solution, an offset from the linearization -/// point. -/// @param threshold The maximum change against the PREVIOUS delta for -/// non-replaced variables that can be ignored, ie. the old delta entry is kept -/// and recursive backsubstitution might eventually stop if none of the changed -/// variables are contained in the subtree. -/// @return The number of variables that were solved for -template -size_t optimizeWildfire(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); +} // namespace gtsam -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); - -/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) -template -int calculate_nnz(const boost::shared_ptr& clique); - -} /// namespace gtsam - -#include #include diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp new file mode 100644 index 000000000..c55ca7959 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -0,0 +1,325 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Clique.cpp + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +// Instantiate base class +template class BayesTreeCliqueBase; + +/* ************************************************************************* */ +void ISAM2Clique::setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult) { + conditional_ = eliminationResult.first; + cachedFactor_ = eliminationResult.second; + // Compute gradient contribution + gradientContribution_.resize(conditional_->cols() - 1); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed + // reasons + gradientContribution_ << -conditional_->get_R().transpose() * + conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); +} + +/* ************************************************************************* */ +bool ISAM2Clique::equals(const This& other, double tol) const { + return Base::equals(other) && + ((!cachedFactor_ && !other.cachedFactor_) || + (cachedFactor_ && other.cachedFactor_ && + cachedFactor_->equals(*other.cachedFactor_, tol))); +} + +/* ************************************************************************* */ +void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { + Base::print(s, formatter); + if (cachedFactor_) + cachedFactor_->print(s + "Cached: ", formatter); + else + cout << s << "Cached empty" << endl; + if (gradientContribution_.rows() != 0) + gtsam::print(gradientContribution_, "Gradient contribution: "); +} + +/* ************************************************************************* */ +bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed + + // Are any clique variables part of the tree that has been redone? + bool dirty = replaced.exists(conditional_->frontals().front()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (Key frontal : conditional_->frontals()) { + assert(dirty == replaced.exists(frontal)); + } +#endif + + // If not, then has one of the separator variables changed significantly? + if (!dirty) { + for (Key parent : conditional_->parents()) { + if (changed.exists(parent)) { + dirty = true; + break; + } + } + } + return dirty; +} + +/* ************************************************************************* */ +/** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ +void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + + // Create solution part pointers if necessary and possible - necessary if + // solnPointers_ is empty, and possible if either we're a root, or we have + // a parent with valid solnPointers_. + ISAM2Clique::shared_ptr parent = parent_.lock(); + if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) { + for (Key frontal : conditional_->frontals()) + solnPointers_.emplace(frontal, delta->find(frontal)); + for (Key parentKey : conditional_->parents()) { + assert(parent->solnPointers_.exists(parentKey)); + solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); + } + } + + // See if we can use solution part pointers - we can if they either + // already existed or were created above. + if (!solnPointers_.empty()) { + GaussianConditional& c = *conditional_; + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(conditional_->nrParents()); + for (Key parent : conditional_->parents()) { + parentPointers.push_back(solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + for (const VectorValues::const_iterator& parentPointer : parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos, 0, parentVector.size(), 1) = + parentVector.block(0, 0, parentVector.size(), 1); + vectorPos += parentVector.size(); + } + } + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = c.get_R().triangularView().solve(rhs); + + // Check for indeterminant solution + if (solution.hasNaN()) + throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for (GaussianConditional::const_iterator frontal = c.beginFrontals(); + frontal != c.endFrontals(); ++frontal) { + solnPointers_.at(*frontal)->second = + solution.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } else { + // Just call plain solve because we couldn't use solution pointers. + delta->update(conditional_->solve(*delta)); + } +#else + delta->update(conditional_->solve(*delta)); +#endif +} + +/* ************************************************************************* */ +bool ISAM2Clique::valuesChanged(const KeySet& replaced, + const Vector& originalValues, + const VectorValues& delta, + double threshold) const { + auto frontals = conditional_->frontals(); + if (replaced.exists(frontals.front())) return true; + auto diff = originalValues - delta.vector(frontals); + return diff.lpNorm() >= threshold; +} + +/* ************************************************************************* */ +/// Set changed flag for each frontal variable +void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { + for (Key frontal : conditional_->frontals()) { + changed->insert(frontal); + } +} + +/* ************************************************************************* */ +void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const { + size_t pos = 0; + for (Key frontal : conditional_->frontals()) { + auto v = delta->at(frontal); + v = originalValues.segment(pos, v.size()); + pos += v.size(); + } +} + +/* ************************************************************************* */ +// Note: not being used right now in favor of non-recursive version below. +void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + if (isDirty(replaced, *changed)) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + + // Recurse to children + for (const auto& child : children) { + child->optimizeWildfire(replaced, threshold, changed, delta, count); + } + } +} + +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + +/* ************************************************************************* */ +bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + bool dirty = isDirty(replaced, *changed); + if (dirty) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + } + + return dirty; +} + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& keys, + VectorValues* delta) { + KeySet changed; + size_t count = 0; + + if (root) { + std::stack travStack; + travStack.push(root); + ISAM2Clique::shared_ptr currentNode = root; + while (!travStack.empty()) { + currentNode = travStack.top(); + travStack.pop(); + bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, + delta, &count); + if (dirty) { + for (const auto& child : currentNode->children) { + travStack.push(child); + } + } + } + } + + return count; +} + +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + +/* ************************************************************************* */ +void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const { + static const bool debug = false; + // does the separator contain any of the variables? + bool found = false; + for (Key key : conditional()->parents()) { + if (markedMask.exists(key)) { + found = true; + break; + } + } + if (found) { + // then add this clique + keys->insert(conditional()->beginFrontals(), conditional()->endFrontals()); + if (debug) print("Key(s) marked in clique "); + if (debug) cout << "so marking key " << conditional()->front() << endl; + } + for (const auto& child : children) { + child->findAll(markedMask, keys); + } +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h new file mode 100644 index 000000000..3c53e3d72 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Clique.h + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Specialized Clique structure for ISAM2, incorporating caching and gradient + * contribution + * TODO: more documentation + */ +class GTSAM_EXPORT ISAM2Clique + : public BayesTreeCliqueBase { + public: + typedef ISAM2Clique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef GaussianConditional ConditionalType; + typedef ConditionalType::shared_ptr sharedConditional; + + Base::FactorType::shared_ptr cachedFactor_; + Vector gradientContribution_; +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + mutable FastMap solnPointers_; +#endif + + /// Default constructor + ISAM2Clique() : Base() {} + + /// Copy constructor, does *not* copy solution pointers as these are invalid + /// in different trees. + ISAM2Clique(const ISAM2Clique& other) + : Base(other), + cachedFactor_(other.cachedFactor_), + gradientContribution_(other.gradientContribution_) {} + + /// Assignment operator, does *not* copy solution pointers as these are + /// invalid in different trees. + ISAM2Clique& operator=(const ISAM2Clique& other) { + Base::operator=(other); + cachedFactor_ = other.cachedFactor_; + gradientContribution_ = other.gradientContribution_; + return *this; + } + + /// Overridden to also store the remaining factor and gradient contribution + void setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult); + + /** Access the cached factor */ + Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + + /** Access the gradient contribution */ + const Vector& gradientContribution() const { return gradientContribution_; } + + bool equals(const This& other, double tol = 1e-9) const; + + /** print this node */ + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + void optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + bool optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + /** + * Starting from the root, add up entries of frontal and conditional matrices + * of each conditional + */ + void nnz_internal(size_t* result) const; + size_t calculate_nnz() const; + + /** + * Recursively search this clique and its children for marked keys appearing + * in the separator, and add the *frontal* keys of any cliques whose + * separator contains any marked keys to the set \c keys. The purpose of + * this is to discover the cliques that need to be redone due to information + * propagating to them from cliques that directly contain factors being + * relinearized. + * + * The original comment says this finds all variables directly connected to + * the marked ones by measurements. Is this true, because it seems like this + * would also pull in variables indirectly connected through other frontal or + * separator variables? + * + * Alternatively could we trace up towards the root for each variable here? + */ + void findAll(const KeySet& markedMask, KeySet* keys) const; + + private: + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ + void fastBackSubstitute(VectorValues* delta) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, const Vector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const; + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(cachedFactor_); + ar& BOOST_SERIALIZATION_NVP(gradientContribution_); + } +}; // \struct ISAM2Clique + +/** + * Optimize the BayesTree, starting from the root. + * @param threshold The maximum change against the PREVIOUS delta for + * non-replaced variables that can be ignored, ie. the old delta entry is kept + * and recursive backsubstitution might eventually stop if none of the changed + * variables are contained in the subtree. + * @param replaced Needs to contain all variables that are contained in the top + * of the Bayes tree that has been redone. + * @return The number of variables that were solved for. + * @param delta The current solution, an offset from the linearization point. + */ +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& replaced, VectorValues* delta); + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& replaced, + VectorValues* delta); + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.cpp b/gtsam/nonlinear/ISAM2Params.cpp new file mode 100644 index 000000000..c768ce427 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Params.cpp + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string ISAM2DoglegParams::adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const { + string s; + switch (adaptationMode) { + case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: + s = "SEARCH_EACH_ITERATION"; + break; + case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: + s = "ONE_STEP_PER_ITERATION"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +/* ************************************************************************* */ +DoglegOptimizerImpl::TrustRegionAdaptationMode +ISAM2DoglegParams::adaptationModeTranslator( + const string& adaptationMode) const { + string s = adaptationMode; + boost::algorithm::to_upper(s); + if (s == "SEARCH_EACH_ITERATION") + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; + if (s == "ONE_STEP_PER_ITERATION") + return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; + + /* default is SEARCH_EACH_ITERATION */ + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; +} + +/* ************************************************************************* */ +ISAM2Params::Factorization ISAM2Params::factorizationTranslator( + const string& str) { + string s = str; + boost::algorithm::to_upper(s); + if (s == "QR") return ISAM2Params::QR; + if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; + + /* default is CHOLESKY */ + return ISAM2Params::CHOLESKY; +} + +/* ************************************************************************* */ +string ISAM2Params::factorizationTranslator( + const ISAM2Params::Factorization& value) { + string s; + switch (value) { + case ISAM2Params::QR: + s = "QR"; + break; + case ISAM2Params::CHOLESKY: + s = "CHOLESKY"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h new file mode 100644 index 000000000..afddd1f8e --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.h @@ -0,0 +1,365 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Params.h + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or + * ISAM2DoglegParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2GaussNewtonParams { + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 0.001) + + /** Specify parameters as constructor arguments */ + ISAM2GaussNewtonParams( + double _wildfireThreshold = + 0.001 ///< see ISAM2GaussNewtonParams public variables, + ///< ISAM2GaussNewtonParams::wildfireThreshold + ) + : wildfireThreshold(_wildfireThreshold) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2GaussNewtonParams\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout.flush(); + } + + double getWildfireThreshold() const { return wildfireThreshold; } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } +}; + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Dogleg optimization. Either this class or + * ISAM2GaussNewtonParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2DoglegParams { + double initialDelta; ///< The initial trust region radius for Dogleg + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 1e-5) + DoglegOptimizerImpl::TrustRegionAdaptationMode + adaptationMode; ///< See description in + ///< DoglegOptimizerImpl::TrustRegionAdaptationMode + bool + verbose; ///< Whether Dogleg prints iteration and convergence information + + /** Specify parameters as constructor arguments */ + ISAM2DoglegParams( + double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta + double _wildfireThreshold = + 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold + DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = + DoglegOptimizerImpl:: + SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode + bool _verbose = false ///< see ISAM2DoglegParams::verbose + ) + : initialDelta(_initialDelta), + wildfireThreshold(_wildfireThreshold), + adaptationMode(_adaptationMode), + verbose(_verbose) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2DoglegParams\n"; + cout << str << "initialDelta: " << initialDelta << "\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout << str + << "adaptationMode: " << adaptationModeTranslator(adaptationMode) + << "\n"; + cout.flush(); + } + + double getInitialDelta() const { return initialDelta; } + double getWildfireThreshold() const { return wildfireThreshold; } + std::string getAdaptationMode() const { + return adaptationModeTranslator(adaptationMode); + } + bool isVerbose() const { return verbose; } + void setInitialDelta(double initialDelta) { + this->initialDelta = initialDelta; + } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } + void setAdaptationMode(const std::string& adaptationMode) { + this->adaptationMode = adaptationModeTranslator(adaptationMode); + } + void setVerbose(bool verbose) { this->verbose = verbose; } + + std::string adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const; + DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( + const std::string& adaptationMode) const; +}; + +/** + * @addtogroup ISAM2 + * Parameters for the ISAM2 algorithm. Default parameter values are listed + * below. + */ +typedef FastMap ISAM2ThresholdMap; +typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; +struct GTSAM_EXPORT ISAM2Params { + typedef boost::variant + OptimizationParams; ///< Either ISAM2GaussNewtonParams or + ///< ISAM2DoglegParams + typedef boost::variant > + RelinearizationThreshold; ///< Either a constant relinearization + ///< threshold or a per-variable-type set of + ///< thresholds + + /** Optimization parameters, this both selects the nonlinear optimization + * method and specifies its parameters, either ISAM2GaussNewtonParams or + * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used + * with the specified parameters, and in the latter Powell's dog-leg + * algorithm will be used with the specified parameters. + */ + OptimizationParams optimizationParams; + + /** Only relinearize variables whose linear delta magnitude is greater than + * this threshold (default: 0.1). If this is a FastMap instead + * of a double, then the threshold is specified for each dimension of each + * variable type. This parameter then maps from a character indicating the + * variable type to a Vector of thresholds for each dimension of that + * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, + * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate + * entries would be added with: + * \code + FastMap thresholds; + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); + // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] = + Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold + params.relinearizeThreshold = thresholds; + \endcode + */ + RelinearizationThreshold relinearizeThreshold; + + int relinearizeSkip; ///< Only relinearize any variables every + ///< relinearizeSkip calls to ISAM2::update (default: + ///< 10) + + bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize + ///< any variables (default: true) + + bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error + ///< before and after the update, to return in + ///< ISAM2Result from update() + + enum Factorization { CHOLESKY, QR }; + /** Specifies whether to use QR or CHOESKY numerical factorization (default: + * CHOLESKY). Cholesky is faster but potentially numerically unstable for + * poorly-conditioned problems, which can occur when uncertainty is very low + * in some variables (or dimensions of variables) and very high in others. QR + * is slower but more numerically stable in poorly-conditioned problems. We + * suggest using the default of Cholesky unless gtsam sometimes throws + * IndefiniteLinearSystemException when your problem's Hessian is actually + * positive definite. For positive definite problems, numerical error + * accumulation can cause the problem to become numerically negative or + * indefinite as solving proceeds, especially when using Cholesky. + */ + Factorization factorization; + + /** Whether to cache linear factors (default: true). + * This can improve performance if linearization is expensive, but can hurt + * performance if linearization is very cleap due to computation to look up + * additional keys. + */ + bool cacheLinearizedFactors; + + KeyFormatter + keyFormatter; ///< A KeyFormatter for when keys are printed during + ///< debugging (default: DefaultKeyFormatter) + + bool enableDetailedResults; ///< Whether to compute and return + ///< ISAM2Result::detailedResults, this can + ///< increase running time (default: false) + + /** Check variables for relinearization in tree-order, stopping the check once + * a variable does not need to be relinearized (default: false). This can + * improve speed by only checking a small part of the top of the tree. + * However, variables below the check cut-off can accumulate significant + * deltas without triggering relinearization. This is particularly useful in + * exploration scenarios where real-time performance is desired over + * correctness. Use with caution. + */ + bool enablePartialRelinearizationCheck; + + /// When you will be removing many factors, e.g. when using ISAM2 as a + /// fixed-lag smoother, enable this option to add factors in the first + /// available factor slots, to avoid accumulating NULL factor slots, at the + /// cost of having to search for slots every time a factor is added. + bool findUnusedFactorSlots; + + /** + * Specify parameters as constructor arguments + * See the documentation of member variables above. + */ + ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), + RelinearizationThreshold _relinearizeThreshold = 0.1, + int _relinearizeSkip = 10, bool _enableRelinearization = true, + bool _evaluateNonlinearError = false, + Factorization _factorization = ISAM2Params::CHOLESKY, + bool _cacheLinearizedFactors = true, + const KeyFormatter& _keyFormatter = + DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter + ) + : optimizationParams(_optimizationParams), + relinearizeThreshold(_relinearizeThreshold), + relinearizeSkip(_relinearizeSkip), + enableRelinearization(_enableRelinearization), + evaluateNonlinearError(_evaluateNonlinearError), + factorization(_factorization), + cacheLinearizedFactors(_cacheLinearizedFactors), + keyFormatter(_keyFormatter), + enableDetailedResults(false), + enablePartialRelinearizationCheck(false), + findUnusedFactorSlots(false) {} + + /// print iSAM2 parameters + void print(const std::string& str = "") const { + using std::cout; + cout << str << "\n"; + + static const std::string kStr("optimizationParams: "); + if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + boost::get(optimizationParams).print(); + else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) + boost::get(optimizationParams).print(kStr); + else + cout << kStr << "{unknown type}\n"; + + cout << "relinearizeThreshold: "; + if (relinearizeThreshold.type() == typeid(double)) { + cout << boost::get(relinearizeThreshold) << "\n"; + } else { + cout << "{mapped}\n"; + for (const ISAM2ThresholdMapValue& value : + boost::get(relinearizeThreshold)) { + cout << " '" << value.first + << "' -> [" << value.second.transpose() << " ]\n"; + } + } + + cout << "relinearizeSkip: " << relinearizeSkip << "\n"; + cout << "enableRelinearization: " << enableRelinearization + << "\n"; + cout << "evaluateNonlinearError: " << evaluateNonlinearError + << "\n"; + cout << "factorization: " + << factorizationTranslator(factorization) << "\n"; + cout << "cacheLinearizedFactors: " << cacheLinearizedFactors + << "\n"; + cout << "enableDetailedResults: " << enableDetailedResults + << "\n"; + cout << "enablePartialRelinearizationCheck: " + << enablePartialRelinearizationCheck << "\n"; + cout << "findUnusedFactorSlots: " << findUnusedFactorSlots + << "\n"; + cout.flush(); + } + + /// @name Getters and Setters for all properties + /// @{ + + OptimizationParams getOptimizationParams() const { + return this->optimizationParams; + } + RelinearizationThreshold getRelinearizeThreshold() const { + return relinearizeThreshold; + } + int getRelinearizeSkip() const { return relinearizeSkip; } + bool isEnableRelinearization() const { return enableRelinearization; } + bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } + std::string getFactorization() const { + return factorizationTranslator(factorization); + } + bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } + KeyFormatter getKeyFormatter() const { return keyFormatter; } + bool isEnableDetailedResults() const { return enableDetailedResults; } + bool isEnablePartialRelinearizationCheck() const { + return enablePartialRelinearizationCheck; + } + + void setOptimizationParams(OptimizationParams optimizationParams) { + this->optimizationParams = optimizationParams; + } + void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { + this->relinearizeThreshold = relinearizeThreshold; + } + void setRelinearizeSkip(int relinearizeSkip) { + this->relinearizeSkip = relinearizeSkip; + } + void setEnableRelinearization(bool enableRelinearization) { + this->enableRelinearization = enableRelinearization; + } + void setEvaluateNonlinearError(bool evaluateNonlinearError) { + this->evaluateNonlinearError = evaluateNonlinearError; + } + void setFactorization(const std::string& factorization) { + this->factorization = factorizationTranslator(factorization); + } + void setCacheLinearizedFactors(bool cacheLinearizedFactors) { + this->cacheLinearizedFactors = cacheLinearizedFactors; + } + void setKeyFormatter(KeyFormatter keyFormatter) { + this->keyFormatter = keyFormatter; + } + void setEnableDetailedResults(bool enableDetailedResults) { + this->enableDetailedResults = enableDetailedResults; + } + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck) { + this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + return factorization == CHOLESKY + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; + } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h new file mode 100644 index 000000000..1539d90c4 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Result.h @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Result.h + * @brief Class that stores detailed iSAM2 result. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include + +namespace gtsam { + +typedef FastVector FactorIndices; + +/** + * @addtogroup ISAM2 + * This struct is returned from ISAM2::update() and contains information about + * the update that is useful for determining whether the solution is + * converging, and about how much work was required for the update. See member + * variables for details and information about each entry. + */ +struct GTSAM_EXPORT ISAM2Result { + /** The nonlinear error of all of the factors, \a including new factors and + * variables added during the current call to ISAM2::update(). This error is + * calculated using the following variable values: + * \li Pre-existing variables will be evaluated by combining their + * linearization point before this call to update, with their partial linear + * delta, as computed by ISAM2::calculateEstimate(). + * \li New variables will be evaluated at their initialization points passed + * into the current call to update. + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorBefore; + + /** The nonlinear error of all of the factors computed after the current + * update, meaning that variables above the relinearization threshold + * (ISAM2Params::relinearizeThreshold) have been relinearized and new + * variables have undergone one linear update. Variable values are + * again computed by combining their linearization points with their + * partial linear deltas, by ISAM2::calculateEstimate(). + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorAfter; + + /** The number of variables that were relinearized because their linear + * deltas exceeded the reslinearization threshold + * (ISAM2Params::relinearizeThreshold), combined with any additional + * variables that had to be relinearized because they were involved in + * the same factor as a variable above the relinearization threshold. + * On steps where no relinearization is considered + * (see ISAM2Params::relinearizeSkip), this count will be zero. + */ + size_t variablesRelinearized; + + /** The number of variables that were reeliminated as parts of the Bayes' + * Tree were recalculated, due to new factors. When loop closures occur, + * this count will be large as the new loop-closing factors will tend to + * involve variables far away from the root, and everything up to the root + * will be reeliminated. + */ + size_t variablesReeliminated; + + /** The number of factors that were included in reelimination of the Bayes' + * tree. */ + size_t factorsRecalculated; + + /** The number of cliques in the Bayes' Tree */ + size_t cliques; + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors to ISAM2::update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + FactorIndices newFactorsIndices; + + /** A struct holding detailed results, which must be enabled with + * ISAM2Params::enableDetailedResults. + */ + struct DetailedResults { + /** The status of a single variable, this struct is stored in + * DetailedResults::variableStatus */ + struct VariableStatus { + /** Whether the variable was just reeliminated, due to being relinearized, + * observed, new, or on the path up to the root clique from another + * reeliminated variable. */ + bool isReeliminated; + bool isAboveRelinThreshold; ///< Whether the variable was just + ///< relinearized due to being above the + ///< relinearization threshold + bool isRelinearizeInvolved; ///< Whether the variable was below the + ///< relinearization threshold but was + ///< relinearized by being involved in a + ///< factor with a variable above the + ///< relinearization threshold + bool isRelinearized; /// Whether the variable was relinearized, either by + /// being above the relinearization threshold or by + /// involvement. + bool isObserved; ///< Whether the variable was just involved in new + ///< factors + bool isNew; ///< Whether the variable itself was just added + bool inRootClique; ///< Whether the variable is in the root clique + VariableStatus() + : isReeliminated(false), + isAboveRelinThreshold(false), + isRelinearizeInvolved(false), + isRelinearized(false), + isObserved(false), + isNew(false), + inRootClique(false) {} + }; + + /** The status of each variable during this update, see VariableStatus. + */ + FastMap variableStatus; + }; + + /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See + * Detail for information about the results data stored here. */ + boost::optional detail; + + void print(const std::string str = "") const { + using std::cout; + cout << str << " Reelimintated: " << variablesReeliminated + << " Relinearized: " << variablesRelinearized + << " Cliques: " << cliques << std::endl; + } + + /** Getters and Setters */ + size_t getVariablesRelinearized() const { return variablesRelinearized; } + size_t getVariablesReeliminated() const { return variablesReeliminated; } + size_t getCliques() const { return cliques; } +}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 5e107ea58..3164b360e 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -14,7 +14,12 @@ #include #include #include + +#include #include +#include +#include +#include namespace gtsam { @@ -23,8 +28,7 @@ namespace gtsam { * @addtogroup SLAM */ class SmartRangeFactor: public NoiseModelFactor { -protected: - + protected: struct Circle2 { Circle2(const Point2& p, double r) : center(p), radius(r) { @@ -35,11 +39,10 @@ protected: typedef SmartRangeFactor This; - std::vector measurements_; ///< Range measurements - double variance_; ///< variance on noise - -public: + std::vector measurements_; ///< Range measurements + double variance_; ///< variance on noise + public: /** Default constructor: don't use directly */ SmartRangeFactor() { } @@ -48,7 +51,7 @@ public: * Constructor * @param s standard deviation of range measurement noise */ - SmartRangeFactor(double s) : + explicit SmartRangeFactor(double s) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } @@ -83,9 +86,10 @@ public: /** * Triangulate a point from at least three pose-range pairs * Checks for best pair that includes first point + * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - //gttic_(triangulate); + // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -96,14 +100,14 @@ public: Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional best_circle; + boost::optional bestCircle2; // loop over all circles - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { // distance between circle centers. double d = distance2(circle1.center, it.center); if (d < 1e-9) - continue; // skip circles that are in the same location + continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); @@ -111,23 +115,27 @@ public: // if h is large, the intersections are well defined. if (fh && (!best_fh || fh->y() > best_fh->y())) { best_fh = fh; - best_circle = it; + bestCircle2 = it; } } // use best fh to find actual intersection points + if (bestCircle2 && best_fh) { std::list intersections = circleCircleIntersection( - circle1.center, best_circle->center, best_fh); + circle1.center, bestCircle2->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { error1 += distance2(it.center, p1); error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; - //gttoc_(triangulate); + } else { + throw std::runtime_error("triangulate failed"); + } + // gttoc_(triangulate); } /** @@ -137,19 +145,20 @@ public: boost::optional&> H = boost::none) const { size_t n = size(); if (n < 3) { - if (H) + if (H) { // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) (*H)[j] = Matrix::Zero(3, 1); + } return Z_1x1; } else { Vector error = Z_1x1; // triangulate to get the optimized point - // TODO: Should we have a (better?) variant that does this in relative coordinates ? + // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(x); - // TODO: triangulation should be followed by an optimization given poses + // TODO(dellaert): triangulation should be followed by an optimization given poses // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); @@ -168,8 +177,6 @@ public: return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - }; - -} // \namespace gtsam +} // \namespace gtsam diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f56b458be..d6ca896dd 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -304,7 +304,7 @@ TEST(ISAM2, AddFactorsStep1) FactorIndices actualNewFactorIndices; - ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); + ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); @@ -843,7 +843,7 @@ TEST(ISAM2, calculate_nnz) { ISAM2 isam = createSlamlikeISAM2(); int expected = 241; - int actual = calculate_nnz(isam.roots().front()); + int actual = isam.roots().front()->calculate_nnz(); EXPECT_LONGS_EQUAL(expected, actual); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 9ddbbb1b2..3d30d6880 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,13 +20,12 @@ #include #include #include -#include -#include #include #include #include #include #include +#include #include #include @@ -394,19 +393,20 @@ class IterativeLM: public LevenbergMarquardtOptimizer { /// Solver specific parameters ConjugateGradientParameters cgParams_; + Values initial_; public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, const ConjugateGradientParameters &p, const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : - LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { + LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) { } /// Solve that uses conjugate gradient virtual VectorValues solve(const GaussianFactorGraph &gfg, - const Values& initial, const NonlinearOptimizerParams& params) const { - VectorValues zeros = initial.zeroVectors(); + const NonlinearOptimizerParams& params) const { + VectorValues zeros = initial_.zeroVectors(); return conjugateGradientDescent(gfg, zeros, cgParams_); } }; diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index 783661819..2c3843b37 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -29,7 +29,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { bool file_exists = true; try { existing_contents = file_contents(filename_.c_str(), add_header); - } catch (CantOpenFile) { + } catch (const CantOpenFile& e) { file_exists = false; }