From 7d274315724268c858a269991e6cafafd6c13f1d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 11 Jul 2020 02:52:34 -0400 Subject: [PATCH 01/32] support for landmarks in g2o files --- gtsam/slam/dataset.cpp | 102 ++++++++++++++++++++++++++----- gtsam/slam/dataset.h | 17 +++++- gtsam/slam/tests/testDataset.cpp | 27 +++++++- 3 files changed, 128 insertions(+), 18 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 669935994..cb15f84a8 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -11,7 +11,10 @@ /** * @file dataset.cpp * @date Jan 22, 2010 - * @author Kai Ni, Luca Carlone, Frank Dellaert + * @author Kai Ni + * @author Luca Carlone + * @author Frank Dellaert + * @author Varun Agrawal * @brief utility functions for loading datasets */ @@ -193,7 +196,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, } /* ************************************************************************* */ -boost::optional parseVertex(istream& is, const string& tag) { +boost::optional parseVertexPose(istream& is, const string& tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { Key id; double x, y, yaw; @@ -204,6 +207,18 @@ boost::optional parseVertex(istream& is, const string& tag) { } } +/* ************************************************************************* */ +boost::optional parseVertexLandmark(istream& is, const string& tag) { + if (tag == "VERTEX_XY") { + Key id; + double x, y; + is >> id >> x >> y; + return IndexedLandmark(id, Point2(x, y)); + } else { + return boost::none; + } +} + /* ************************************************************************* */ boost::optional parseEdge(istream& is, const string& tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") @@ -232,12 +247,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, string tag; - // load the poses + // load the poses and landmarks while (!is.eof()) { if (!(is >> tag)) break; - const auto indexed_pose = parseVertex(is, tag); + const auto indexed_pose = parseVertexPose(is, tag); if (indexed_pose) { Key id = indexed_pose->first; @@ -247,6 +262,16 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, initial->insert(id, indexed_pose->second); } + const auto indexed_landmark = parseVertexLandmark(is, tag); + if (indexed_landmark) { + Key id = indexed_landmark->first; + + // optional filter + if (maxID && id >= maxID) + continue; + + initial->insert(id, indexed_landmark->second); + } is.ignore(LINESIZE, '\n'); } is.clear(); /* clears the end-of-file and error flags */ @@ -429,7 +454,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const string& filename) { fstream stream(filename.c_str(), fstream::out); - // save 2D & 3D poses + // save 2D poses for (const auto& key_value : estimate) { auto p = dynamic_cast*>(&key_value.value); if (!p) continue; @@ -438,15 +463,34 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << pose.y() << " " << pose.theta() << endl; } + // save 3D poses for(const auto& key_value: estimate) { - auto p = dynamic_cast*>(&key_value.value); - if (!p) continue; - const Pose3& pose = p->value(); - const Point3 t = pose.translation(); - const auto q = pose.rotation().toQuaternion(); - stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " - << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " - << q.z() << " " << q.w() << endl; + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + const Pose3& pose = p->value(); + const Point3 t = pose.translation(); + const auto q = pose.rotation().toQuaternion(); + stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " + << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; + } + + // save 2D landmarks + for(const auto& key_value: estimate) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + const Point2& point = p->value(); + stream << "VERTEX_XY " << key_value.key << " " << point.x() << " " + << point.y() << endl; + } + + // save 3D landmarks + for(const auto& key_value: estimate) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + const Point3& point = p->value(); + stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " " + << point.y() << " " << point.z() << endl; } // save edges (2D or 3D) @@ -515,6 +559,7 @@ static Rot3 NormalizedRot3(double w, double x, double y, double z) { const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm; return Rot3::Quaternion(f * w, f * x, f * y, f * z); } + /* ************************************************************************* */ std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); @@ -545,6 +590,30 @@ std::map parse3DPoses(const string& filename) { return poses; } +/* ************************************************************************* */ +std::map parse3DLandmarks(const string& filename) { + ifstream is(filename.c_str()); + if (!is) + throw invalid_argument("parse3DLandmarks: can not find file " + filename); + + std::map landmarks; + while (!is.eof()) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "VERTEX_TRACKXYZ") { + Key id; + double x, y, z; + ls >> id >> x >> y >> z; + landmarks.emplace(id, Point3(x, y, z)); + } + } + return landmarks; +} + /* ************************************************************************* */ BetweenFactorPose3s parse3DFactors( const string& filename, @@ -617,11 +686,16 @@ GraphAndValues load3D(const string& filename) { graph->push_back(factor); } - const auto poses = parse3DPoses(filename); Values::shared_ptr initial(new Values); + + const auto poses = parse3DPoses(filename); for (const auto& key_pose : poses) { initial->insert(key_pose.first, key_pose.second); } + const auto landmarks = parse3DLandmarks(filename); + for (const auto& key_landmark : landmarks) { + initial->insert(key_landmark.first, key_landmark.second); + } return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 032799429..a18ae12f6 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -76,6 +76,7 @@ enum KernelFunctionType { /// Return type for auxiliary functions typedef std::pair IndexedPose; +typedef std::pair IndexedLandmark; typedef std::pair, Pose2> IndexedEdge; /** @@ -83,9 +84,18 @@ typedef std::pair, Pose2> IndexedEdge; * @param is input stream * @param tag string parsed from input stream, will only parse if vertex type */ -GTSAM_EXPORT boost::optional parseVertex(std::istream& is, +GTSAM_EXPORT boost::optional parseVertexPose(std::istream& is, const std::string& tag); +/** + * Parse G2O landmark vertex "id x y" + * @param is input stream + * @param tag string parsed from input stream, will only parse if vertex type + */ + +GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& is, + const std::string& tag) + /** * Parse TORO/G2O edge "id1 id2 x y yaw" * @param is input stream @@ -162,9 +172,12 @@ using BetweenFactorPose3s = std::vector::shared_ptr> GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr); -/// Parse vertices in 3D TORO graph file into a map of Pose3s. +/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); +/// Parse landmarks in 3D g2o graph file into a map of Point3s. +GTSAM_EXPORT std::map parse3DLandmarks(const string& filename) + /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 8088ab18a..136b7a93c 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -43,13 +43,13 @@ TEST(dataSet, findExampleDataFile) { } /* ************************************************************************* */ -TEST( dataSet, parseVertex) +TEST( dataSet, parseVertexPose) { const string str = "VERTEX2 1 2.000000 3.000000 4.000000"; istringstream is(str); string tag; EXPECT(is >> tag); - const auto actual = parseVertex(is, tag); + const auto actual = parseVertexPose(is, tag); EXPECT(actual); if (actual) { EXPECT_LONGS_EQUAL(1, actual->first); @@ -57,6 +57,21 @@ TEST( dataSet, parseVertex) } } +/* ************************************************************************* */ +TEST( dataSet, parseVertexLandmark) +{ + const string str = "VERTEX_XY 1 2.000000 3.000000"; + istringstream is(str); + string tag; + EXPECT(is >> tag); + const auto actual = parseVertexLandmark(is, tag); + EXPECT(actual); + if (actual) { + EXPECT_LONGS_EQUAL(1, actual->first); + EXPECT(assert_equal(Point2(2, 3), actual->second)); + } +} + /* ************************************************************************* */ TEST( dataSet, parseEdge) { @@ -182,6 +197,12 @@ TEST(dataSet, readG2o3D) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } + // Check landmark parsing + const auto actualLandmarks = parse3DLandmarks(g2oFile); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + // Check graph version NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; @@ -252,6 +273,8 @@ TEST(dataSet, readG2oCheckDeterminants) { const Rot3 R = key_value.second.rotation(); EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); } + const map landmarks = parse3DLandmarks(g2oFile); + EXPECT_LONGS_EQUAL(0, landmarks.size()); } /* ************************************************************************* */ From 3f065f25c66e3bce14ac0c1206af000ec171b329 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 21 Jul 2020 11:32:58 -0500 Subject: [PATCH 02/32] Added more comments for clearer understanding --- gtsam/navigation/TangentPreintegration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 55efdb151..56d7aa6d3 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -68,7 +68,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, Matrix3 w_tangent_H_theta, invH; const Vector3 w_tangent = // angular velocity mapped back to tangent space local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const Rot3 R(local.expmap()); + const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; @@ -110,7 +110,7 @@ void TangentPreintegration::update(const Vector3& measuredAcc, Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - // Possibly correct for sensor pose + // Possibly correct for sensor pose by converting to body frame Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; if (p().body_P_sensor) boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, From 24b2f50fe374a2d808a44c133c03f4793da02c60 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 21 Jul 2020 18:15:00 -0400 Subject: [PATCH 03/32] Bump version and switch Pose3 expmap default --- CMakeLists.txt | 6 +++--- appveyor.yml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 12cb6882e..edefbf2ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 0) -set (GTSAM_VERSION_PATCH 2) +set (GTSAM_VERSION_PATCH 3) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") @@ -68,8 +68,8 @@ if(GTSAM_UNSTABLE_AVAILABLE) endif() option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) -option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) diff --git a/appveyor.yml b/appveyor.yml index 2c78ca1f2..3747354cf 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 4.0.2-{branch}-build{build} +version: 4.0.3-{branch}-build{build} os: Visual Studio 2019 From 456f1baf8f3083146310b7a45ba4675783dc9c8d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 21 Jul 2020 20:55:21 -0400 Subject: [PATCH 04/32] Fix test for full Pose3 expmap --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index a85118c00..2f4f21286 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -168,7 +168,11 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera); Vector3 X = point; +#ifdef GTSAM_POSE3_EXPMAP +Vector2 expectedMeasurement(1.3124675, 1.2057287); +#else Vector2 expectedMeasurement(1.2431567, 1.2525694); +#endif Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); } @@ -177,7 +181,11 @@ Matrix E2 = numericalDerivative22(adapted, P, X); // Check that Local worked as expected TEST(AdaptAutoDiff, Local) { using namespace example; +#ifdef GTSAM_POSE3_EXPMAP + Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished(); +#else Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished(); +#endif EXPECT(equal_with_abs_tol(expectedP, P)); Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely! EXPECT(equal_with_abs_tol(expectedX, X)); From e993afe2bfe70366685b5395c84c3217ff8e63f7 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 23 Jul 2020 14:05:23 -0400 Subject: [PATCH 05/32] replace boost random with std random --- examples/SolverComparer.cpp | 6 ++-- gtsam/geometry/SO4.cpp | 4 +-- gtsam/geometry/SO4.h | 2 +- timing/timeFactorOverhead.cpp | 13 ++++---- timing/timeFrobeniusFactor.cpp | 3 +- timing/timeMatrixOps.cpp | 61 +++++++++++++++++----------------- 6 files changed, 46 insertions(+), 43 deletions(-) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index d1155fe4c..718ae6286 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -50,11 +50,11 @@ #include #include #include -#include #include #include #include +#include #ifdef GTSAM_USE_TBB #include // tbb::task_arena @@ -554,8 +554,8 @@ void runCompare() void runPerturb() { // Set up random number generator - boost::mt19937 rng; - boost::normal_distribution normal(0.0, perturbationNoise); + std::mt19937 rng; + std::normal_distribution normal(0.0, perturbationNoise); // Perturb values VectorValues noise; diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 8a78bb83f..1c4920af8 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -35,7 +35,7 @@ namespace gtsam { // TODO(frank): is this better than SOn::Random? // /* ************************************************************************* -// */ static Vector3 randomOmega(boost::mt19937 &rng) { +// */ static Vector3 randomOmega(std::mt19937 &rng) { // static std::uniform_real_distribution randomAngle(-M_PI, M_PI); // return Unit3::Random(rng).unitVector() * randomAngle(rng); // } @@ -43,7 +43,7 @@ namespace gtsam { // /* ************************************************************************* // */ // // Create random SO(4) element using direct product of lie algebras. -// SO4 SO4::Random(boost::mt19937 &rng) { +// SO4 SO4::Random(std::mt19937 &rng) { // Vector6 delta; // delta << randomOmega(rng), randomOmega(rng); // return SO4::Expmap(delta); diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 5014414c2..33bd8c161 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -34,7 +34,7 @@ namespace gtsam { using SO4 = SO<4>; // /// Random SO(4) element (no big claims about uniformity) -// static SO4 Random(boost::mt19937 &rng); +// static SO4 Random(std::mt19937 &rng); // Below are all declarations of SO<4> specializations. // They are *defined* in SO4.cpp. diff --git a/timing/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp index d9c19703c..717a2f434 100644 --- a/timing/timeFactorOverhead.cpp +++ b/timing/timeFactorOverhead.cpp @@ -22,13 +22,14 @@ #include #include -#include +#include #include using namespace gtsam; using namespace std; -static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); +static std::mt19937 rng; +static std::uniform_real_distribution<> rg(0.0, 1.0); int main(int argc, char *argv[]) { @@ -64,10 +65,10 @@ int main(int argc, char *argv[]) { Matrix A(blockdim, vardim); for(size_t j=0; j(key, A, b, noise)); } } @@ -111,10 +112,10 @@ int main(int argc, char *argv[]) { // Generate a random Gaussian factor for(size_t j=0; j(key, Acomb, bcomb, noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))); diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp index c8bdd8fec..30ebff7bc 100644 --- a/timing/timeFrobeniusFactor.cpp +++ b/timing/timeFrobeniusFactor.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -76,7 +77,7 @@ int main(int argc, char* argv[]) { keys[0], keys[1], SO3(Tij.rotation().matrix()), model); } - boost::mt19937 rng(42); + std::mt19937 rng(42); // Set parameters to be similar to ceres LevenbergMarquardtParams params; diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index d9f2ffaf6..8dcb2bbc9 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -16,7 +16,6 @@ * @date Sep 18, 2010 */ -#include #include #include @@ -24,6 +23,7 @@ #include #include +#include #include #include @@ -33,7 +33,8 @@ using namespace std; using boost::format; using namespace boost::lambda; -static boost::variate_generator > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0)); +static std::mt19937 rng; +static std::uniform_real_distribution<> rg(-1.0, 0.0); //typedef ublas::matrix matrix; //typedef ublas::matrix_range matrix_range; //typedef Eigen::Matrix matrix; @@ -53,8 +54,8 @@ int main(int argc, char* argv[]) { volatile size_t n=300; volatile size_t nReps = 1000; assert(m > n); - boost::variate_generator > rni(boost::mt19937(), boost::uniform_int(0,m-1)); - boost::variate_generator > rnj(boost::mt19937(), boost::uniform_int(0,n-1)); + std::uniform_int_distribution ri(0,m-1); + std::uniform_int_distribution rj(0,n-1); gtsam::Matrix mat((int)m,(int)n); gtsam::SubMatrix full = mat.block(0, 0, m, n); gtsam::SubMatrix top = mat.block(0, 0, n, n); @@ -75,13 +76,13 @@ int main(int argc, char* argv[]) { for(size_t rep=0; rep<1000; ++rep) for(size_t i=0; i<(size_t)mat.rows(); ++i) for(size_t j=0; j<(size_t)mat.cols(); ++j) - mat(i,j) = rng(); + mat(i,j) = rg(rng); gttic_(basicTime); for(size_t rep=0; repsecs(); @@ -92,7 +93,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -103,7 +104,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -114,7 +115,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -133,13 +134,13 @@ int main(int argc, char* argv[]) { for(size_t rep=0; rep<1000; ++rep) for(size_t j=0; j<(size_t)mat.cols(); ++j) for(size_t i=0; i<(size_t)mat.rows(); ++i) - mat(i,j) = rng(); + mat(i,j) = rg(rng); gttic_(basicTime); for(size_t rep=0; repsecs(); @@ -150,7 +151,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -161,7 +162,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -172,7 +173,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -190,14 +191,14 @@ int main(int argc, char* argv[]) { cout << "Row-major matrix, random assignment:" << endl; // Do a few initial assignments to let any cache effects stabilize - for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng),rj(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rg(rng); } gttic_(basicTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng),rj(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rg(rng); } gttoc_(basicTime); tictoc_getNode(basicTimeNode, basicTime); basicTime = basicTimeNode->secs(); @@ -205,9 +206,9 @@ int main(int argc, char* argv[]) { cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl; gttic_(fullTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng),rj(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rg(rng); } gttoc_(fullTime); tictoc_getNode(fullTimeNode, fullTime); fullTime = fullTimeNode->secs(); @@ -215,9 +216,9 @@ int main(int argc, char* argv[]) { cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; gttic_(topTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj())); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng)%top.rows(),rj(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rg(rng); } gttoc_(topTime); tictoc_getNode(topTimeNode, topTime); topTime = topTimeNode->secs(); @@ -225,9 +226,9 @@ int main(int argc, char* argv[]) { cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; gttic_(blockTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols())); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng)%block.rows(),rj(rng)%block.cols())); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rg(rng); } gttoc_(blockTime); tictoc_getNode(blockTimeNode, blockTime); blockTime = blockTimeNode->secs(); @@ -250,7 +251,7 @@ int main(int argc, char* argv[]) { // matrix mat(5,5); // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rng(); +// mat(i,j) = rg(rng); // // tri = ublas::triangular_adaptor(mat); // cout << " Assigned from triangular adapter: " << tri << endl; @@ -259,13 +260,13 @@ int main(int argc, char* argv[]) { // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rng(); +// mat(i,j) = rg(rng); // mat = tri; // cout << " Assign matrix from triangular: " << mat << endl; // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rng(); +// mat(i,j) = rg(rng); // (ublas::triangular_adaptor(mat)) = tri; // cout << " Assign triangular adaptor from triangular: " << mat << endl; // } @@ -281,7 +282,7 @@ int main(int argc, char* argv[]) { // matrix mat(5,7); // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rng(); +// mat(i,j) = rg(rng); // // tri = ublas::triangular_adaptor(mat); // cout << " Assigned from triangular adapter: " << tri << endl; @@ -290,13 +291,13 @@ int main(int argc, char* argv[]) { // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rng(); +// mat(i,j) = rg(rng); // mat = tri; // cout << " Assign matrix from triangular: " << mat << endl; // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rng(); +// mat(i,j) = rg(rng); // mat = ublas::triangular_adaptor(mat); // cout << " Assign matrix from triangular adaptor of self: " << mat << endl; // } From 2e0f2962c4e8a2f81f831eec0af715e7edf8c42f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 23 Jul 2020 13:20:57 -0500 Subject: [PATCH 06/32] add axis labels to the trajectory plot --- cython/gtsam/utils/plot.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index b67444fc1..93d8ba47b 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -267,6 +267,10 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None): plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) + axes.set_xlabel('X axis') + axes.set_ylabel('Y axis') + axes.set_zlabel('Z axis') + def plot_trajectory(fignum, values, scale=1, marginals=None): """ From d5d58fd707e0210ea3401f67f021c9a55848c551 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 23 Jul 2020 14:49:13 -0400 Subject: [PATCH 07/32] use reasonable distribution names --- timing/timeMatrixOps.cpp | 58 ++++++++++++++++++++-------------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index 8dcb2bbc9..95333fbf8 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -34,7 +34,7 @@ using boost::format; using namespace boost::lambda; static std::mt19937 rng; -static std::uniform_real_distribution<> rg(-1.0, 0.0); +static std::uniform_real_distribution<> uniform(-1.0, 0.0); //typedef ublas::matrix matrix; //typedef ublas::matrix_range matrix_range; //typedef Eigen::Matrix matrix; @@ -54,8 +54,8 @@ int main(int argc, char* argv[]) { volatile size_t n=300; volatile size_t nReps = 1000; assert(m > n); - std::uniform_int_distribution ri(0,m-1); - std::uniform_int_distribution rj(0,n-1); + std::uniform_int_distribution uniform_i(0,m-1); + std::uniform_int_distribution uniform_j(0,n-1); gtsam::Matrix mat((int)m,(int)n); gtsam::SubMatrix full = mat.block(0, 0, m, n); gtsam::SubMatrix top = mat.block(0, 0, n, n); @@ -76,13 +76,13 @@ int main(int argc, char* argv[]) { for(size_t rep=0; rep<1000; ++rep) for(size_t i=0; i<(size_t)mat.rows(); ++i) for(size_t j=0; j<(size_t)mat.cols(); ++j) - mat(i,j) = rg(rng); + mat(i,j) = uniform(rng); gttic_(basicTime); for(size_t rep=0; repsecs(); @@ -93,7 +93,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -104,7 +104,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -115,7 +115,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -134,13 +134,13 @@ int main(int argc, char* argv[]) { for(size_t rep=0; rep<1000; ++rep) for(size_t j=0; j<(size_t)mat.cols(); ++j) for(size_t i=0; i<(size_t)mat.rows(); ++i) - mat(i,j) = rg(rng); + mat(i,j) = uniform(rng); gttic_(basicTime); for(size_t rep=0; repsecs(); @@ -151,7 +151,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -162,7 +162,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -173,7 +173,7 @@ int main(int argc, char* argv[]) { for(size_t rep=0; repsecs(); @@ -191,14 +191,14 @@ int main(int argc, char* argv[]) { cout << "Row-major matrix, random assignment:" << endl; // Do a few initial assignments to let any cache effects stabilize - for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng),rj(rng))); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rg(rng); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = uniform(rng); } gttic_(basicTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng),rj(rng))); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rg(rng); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = uniform(rng); } gttoc_(basicTime); tictoc_getNode(basicTimeNode, basicTime); basicTime = basicTimeNode->secs(); @@ -206,9 +206,9 @@ int main(int argc, char* argv[]) { cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl; gttic_(fullTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng),rj(rng))); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rg(rng); } + for(const ij_t& ij: ijs) { full(ij.first, ij.second) = uniform(rng); } gttoc_(fullTime); tictoc_getNode(fullTimeNode, fullTime); fullTime = fullTimeNode->secs(); @@ -216,9 +216,9 @@ int main(int argc, char* argv[]) { cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; gttic_(topTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng)%top.rows(),rj(rng))); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng)%top.rows(),uniform_j(rng))); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rg(rng); } + for(const ij_t& ij: ijs) { top(ij.first, ij.second) = uniform(rng); } gttoc_(topTime); tictoc_getNode(topTimeNode, topTime); topTime = topTimeNode->secs(); @@ -226,9 +226,9 @@ int main(int argc, char* argv[]) { cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; gttic_(blockTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(ri(rng)%block.rows(),rj(rng)%block.cols())); + for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng)%block.rows(),uniform_j(rng)%block.cols())); for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rg(rng); } + for(const ij_t& ij: ijs) { block(ij.first, ij.second) = uniform(rng); } gttoc_(blockTime); tictoc_getNode(blockTimeNode, blockTime); blockTime = blockTimeNode->secs(); @@ -251,7 +251,7 @@ int main(int argc, char* argv[]) { // matrix mat(5,5); // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rg(rng); +// mat(i,j) = uniform(rng); // // tri = ublas::triangular_adaptor(mat); // cout << " Assigned from triangular adapter: " << tri << endl; @@ -260,13 +260,13 @@ int main(int argc, char* argv[]) { // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rg(rng); +// mat(i,j) = uniform(rng); // mat = tri; // cout << " Assign matrix from triangular: " << mat << endl; // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rg(rng); +// mat(i,j) = uniform(rng); // (ublas::triangular_adaptor(mat)) = tri; // cout << " Assign triangular adaptor from triangular: " << mat << endl; // } @@ -282,7 +282,7 @@ int main(int argc, char* argv[]) { // matrix mat(5,7); // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rg(rng); +// mat(i,j) = uniform(rng); // // tri = ublas::triangular_adaptor(mat); // cout << " Assigned from triangular adapter: " << tri << endl; @@ -291,13 +291,13 @@ int main(int argc, char* argv[]) { // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rg(rng); +// mat(i,j) = uniform(rng); // mat = tri; // cout << " Assign matrix from triangular: " << mat << endl; // // for(size_t j=0; j<(size_t)mat.cols(); ++j) // for(size_t i=0; i<(size_t)mat.rows(); ++i) -// mat(i,j) = rg(rng); +// mat(i,j) = uniform(rng); // mat = ublas::triangular_adaptor(mat); // cout << " Assign matrix from triangular adaptor of self: " << mat << endl; // } From 5eb2bea3572790fe73f6771e0121e148ebf0f66f Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 23 Jul 2020 14:54:21 -0400 Subject: [PATCH 08/32] use reasonable distribution name --- timing/timeFactorOverhead.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/timing/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp index 717a2f434..038901388 100644 --- a/timing/timeFactorOverhead.cpp +++ b/timing/timeFactorOverhead.cpp @@ -29,7 +29,7 @@ using namespace gtsam; using namespace std; static std::mt19937 rng; -static std::uniform_real_distribution<> rg(0.0, 1.0); +static std::uniform_real_distribution<> uniform(0.0, 1.0); int main(int argc, char *argv[]) { @@ -65,10 +65,10 @@ int main(int argc, char *argv[]) { Matrix A(blockdim, vardim); for(size_t j=0; j(key, A, b, noise)); } } @@ -112,10 +112,10 @@ int main(int argc, char *argv[]) { // Generate a random Gaussian factor for(size_t j=0; j(key, Acomb, bcomb, noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))); From c738f6b7f727b79b63596677bee5a0ffb578b00c Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 23 Jul 2020 15:47:14 -0400 Subject: [PATCH 09/32] remove unused base variable --- gtsam/nonlinear/FunctorizedFactor.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index a83198967..5351b126f 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -109,7 +109,6 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const { const FunctorizedFactor *e = dynamic_cast *>(&other); - const bool base = Base::equals(*e, tol); return e && Base::equals(other, tol) && traits::Equals(this->measured_, e->measured_, tol); } From aaf632f583090cbacc9f6683538d68ab0cd79152 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 02:30:35 -0500 Subject: [PATCH 10/32] fix test for FunctorizedFactor printing --- gtsam/nonlinear/tests/testFunctorizedFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 12dd6b91c..48ab73ad0 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -131,7 +131,7 @@ TEST(FunctorizedFactor, Print) { "FunctorizedFactor(X0)\n" " measurement: [\n" " 1, 0;\n" - " 0, 1\n" + " 0, 1\n" "]\n" " noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; From 05f50eae40842abed479164d39903be905396692 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 03:10:03 -0500 Subject: [PATCH 11/32] Fix minor bugs --- gtsam/slam/dataset.h | 4 ++-- tests/testNonlinearISAM.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a18ae12f6..439a69fdc 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -94,7 +94,7 @@ GTSAM_EXPORT boost::optional parseVertexPose(std::istream& is, */ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& is, - const std::string& tag) + const std::string& tag); /** * Parse TORO/G2O edge "id1 id2 x y yaw" @@ -176,7 +176,7 @@ GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); /// Parse landmarks in 3D g2o graph file into a map of Point3s. -GTSAM_EXPORT std::map parse3DLandmarks(const string& filename) +GTSAM_EXPORT std::map parse3DLandmarks(const string& filename); /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 88ae508b6..974806612 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -288,7 +288,7 @@ TEST(testNonlinearISAM, loop_closures ) { break; // Check if vertex - const auto indexedPose = parseVertex(is, tag); + const auto indexedPose = parseVertexPose(is, tag); if (indexedPose) { Key id = indexedPose->first; initialEstimate.insert(Symbol('x', id), indexedPose->second); From 8a210188f3923fa8007168d0ba941fe450fdef9e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 03:10:14 -0500 Subject: [PATCH 12/32] test for readG2o --- examples/Data/example_with_vertices.g2o | 16 ++++++++++++++++ gtsam/slam/tests/testDataset.cpp | 11 +++++++++++ 2 files changed, 27 insertions(+) create mode 100644 examples/Data/example_with_vertices.g2o diff --git a/examples/Data/example_with_vertices.g2o b/examples/Data/example_with_vertices.g2o new file mode 100644 index 000000000..6c2f1a530 --- /dev/null +++ b/examples/Data/example_with_vertices.g2o @@ -0,0 +1,16 @@ +VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162 +VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508 +VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10 +VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508 +VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162 +VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567 +VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412 +VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567 +VERTEX_TRACKXYZ 7782220156096217088 10 10 10 +VERTEX_TRACKXYZ 7782220156096217089 -10 10 10 +VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10 +VERTEX_TRACKXYZ 7782220156096217091 10 -10 10 +VERTEX_TRACKXYZ 7782220156096217092 10 10 -10 +VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10 +VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10 +VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10 diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 136b7a93c..b6b1776d2 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -277,6 +277,17 @@ TEST(dataSet, readG2oCheckDeterminants) { EXPECT_LONGS_EQUAL(0, landmarks.size()); } +/* ************************************************************************* */ +TEST(dataSet, readG2oLandmarks) { + const string g2oFile = findExampleDataFile("example_with_vertices.g2o"); + + // Check number of poses and landmarks. Should be 8 each. + const map poses = parse3DPoses(g2oFile); + EXPECT_LONGS_EQUAL(8, poses.size()); + const map landmarks = parse3DLandmarks(g2oFile); + EXPECT_LONGS_EQUAL(8, landmarks.size()); +} + /* ************************************************************************* */ static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) { NonlinearFactorGraph g; From 6660e2a532a76656e75507cdb10e46a12b8c4161 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 09:43:37 -0500 Subject: [PATCH 13/32] added axis labels and figure titles as optional params --- cython/gtsam/utils/plot.py | 68 ++++++++++++++++++++++++++++++-------- 1 file changed, 54 insertions(+), 14 deletions(-) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 93d8ba47b..1e976a69e 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -135,7 +135,8 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): axes.add_patch(e1) -def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): +def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): """ Plot a 2D pose on given figure with given `axis_length`. @@ -144,6 +145,7 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): pose (gtsam.Pose2): The pose to be plotted. axis_length (float): The length of the camera axes. covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. """ # get figure object fig = plt.figure(fignum) @@ -151,6 +153,12 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): plot_pose2_on_axes(axes, pose, axis_length=axis_length, covariance=covariance) + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) + + return fig + def plot_point3_on_axes(axes, point, linespec, P=None): """ @@ -167,7 +175,8 @@ def plot_point3_on_axes(axes, point, linespec, P=None): plot_covariance_ellipse_3d(axes, point.vector(), P) -def plot_point3(fignum, point, linespec, P=None): +def plot_point3(fignum, point, linespec, P=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): """ Plot a 3D point on given figure with given `linespec`. @@ -176,13 +185,25 @@ def plot_point3(fignum, point, linespec, P=None): point (gtsam.Point3): The point to be plotted. linespec (string): String representing formatting options for Matplotlib. P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + + Returns: + fig: The matplotlib figure. + """ fig = plt.figure(fignum) axes = fig.gca(projection='3d') plot_point3_on_axes(axes, point, linespec, P) + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) -def plot_3d_points(fignum, values, linespec="g*", marginals=None): + return fig + + +def plot_3d_points(fignum, values, linespec="g*", marginals=None, + title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): """ Plots the Point3s in `values`, with optional covariances. Finds all the Point3 objects in the given Values object and plots them. @@ -193,7 +214,9 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None): fignum (int): Integer representing the figure number to use for plotting. values (gtsam.Values): Values dictionary consisting of points to be plotted. linespec (string): String representing formatting options for Matplotlib. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + title (string): The title of the plot. + axis_labels (iterable[string]): List of axis labels to set. """ keys = values.keys() @@ -208,12 +231,16 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None): else: covariance = None - plot_point3(fignum, point, linespec, covariance) + fig = plot_point3(fignum, point, linespec, covariance, + axis_labels=axis_labels) except RuntimeError: continue # I guess it's not a Point3 + fig.suptitle(title) + fig.canvas.set_window_title(title.lower()) + def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): """ @@ -251,7 +278,8 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): plot_covariance_ellipse_3d(axes, origin, gPp) -def plot_pose3(fignum, pose, axis_length=0.1, P=None): +def plot_pose3(fignum, pose, axis_length=0.1, P=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): """ Plot a 3D pose on given figure with given `axis_length`. @@ -260,6 +288,10 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None): pose (gtsam.Pose3): 3D pose to be plotted. linespec (string): String representing formatting options for Matplotlib. P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + + Returns: + fig: The matplotlib figure. """ # get figure object fig = plt.figure(fignum) @@ -267,12 +299,15 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None): plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) - axes.set_xlabel('X axis') - axes.set_ylabel('Y axis') - axes.set_zlabel('Z axis') + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) + + return fig -def plot_trajectory(fignum, values, scale=1, marginals=None): +def plot_trajectory(fignum, values, scale=1, marginals=None, + title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')): """ Plot a complete 3D trajectory using poses in `values`. @@ -282,6 +317,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): scale (float): Value to scale the poses by. marginals (gtsam.Marginals): Marginalized probability values of the estimation. Used to plot uncertainty bounds. + title (string): The title of the plot. + axis_labels (iterable[string]): List of axis labels to set. """ pose3Values = gtsam.utilities_allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) @@ -307,8 +344,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): else: covariance = None - plot_pose3(fignum, lastPose, P=covariance, - axis_length=scale) + fig = plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale, axis_labels=axis_labels) lastIndex = i @@ -322,8 +359,11 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): else: covariance = None - plot_pose3(fignum, lastPose, P=covariance, - axis_length=scale) + fig = plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale, axis_labels=axis_labels) except: pass + + fig.suptitle(title) + fig.canvas.set_window_title(title.lower()) From a7afd9da70d04d3de7dbd11a473b8a55a365a727 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 11:24:46 -0500 Subject: [PATCH 14/32] increase number of build cores and remove sudo requirement --- .travis.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index d8094ef4d..0c8c4bee5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,5 @@ language: cpp cache: ccache -sudo: required dist: xenial addons: @@ -33,7 +32,7 @@ stages: env: global: - - MAKEFLAGS="-j2" + - MAKEFLAGS="-j3" - CCACHE_SLOPPINESS=pch_defines,time_macros # Compile stage without building examples/tests to populate the caches. From d2063d928e855a326af6e806398b8b2afd97fd6e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 11:37:58 -0500 Subject: [PATCH 15/32] added backwards compatibility for parseVertex --- gtsam/slam/dataset.cpp | 7 +++++++ gtsam/slam/dataset.h | 10 ++++++++++ 2 files changed, 17 insertions(+) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index cb15f84a8..4a95b4297 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -195,6 +195,13 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, } } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +/* ************************************************************************* */ +boost::optional parseVertex(istream& is, const string& tag) { + return parseVertexPose(is, tag); +} +#endif + /* ************************************************************************* */ boost::optional parseVertexPose(istream& is, const string& tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 439a69fdc..7029a7a9c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -79,6 +79,16 @@ typedef std::pair IndexedPose; typedef std::pair IndexedLandmark; typedef std::pair, Pose2> IndexedEdge; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +/** + * Parse TORO/G2O vertex "id x y yaw" + * @param is input stream + * @param tag string parsed from input stream, will only parse if vertex type + */ +GTSAM_EXPORT boost::optional parseVertex(std::istream& is, + const std::string& tag); +#endif + /** * Parse TORO/G2O vertex "id x y yaw" * @param is input stream From 15f581ce29157538f17ef9baf11e12d758e5ea6d Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Fri, 24 Jul 2020 16:40:18 -0400 Subject: [PATCH 16/32] pass in expected Rot3 and parameter p --- timing/timeFrobeniusFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp index 30ebff7bc..8bd754de6 100644 --- a/timing/timeFrobeniusFactor.cpp +++ b/timing/timeFrobeniusFactor.cpp @@ -74,7 +74,7 @@ int main(int argc, char* argv[]) { const auto& Tij = factor->measured(); const auto& model = factor->noiseModel(); graph.emplace_shared( - keys[0], keys[1], SO3(Tij.rotation().matrix()), model); + keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model); } std::mt19937 rng(42); From a4b95d273f7af08387afea7d8dcd6f89cf9014cd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sat, 25 Jul 2020 18:07:25 +0200 Subject: [PATCH 17/32] recover SLAM serialization test --- gtsam/linear/GaussianISAM.h | 5 +++ gtsam/nonlinear/PriorFactor.h | 5 +++ tests/testSerializationSLAM.cpp | 71 +-------------------------------- 3 files changed, 11 insertions(+), 70 deletions(-) diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index 55208d4d1..b77b26240 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -43,4 +44,8 @@ namespace gtsam { }; + /// traits + template <> + struct traits : public Testable {}; + } diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 0afbaa588..3198dab07 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -117,4 +117,9 @@ namespace gtsam { GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; + /// traits + template + struct traits > : public Testable > {}; + + } /// namespace gtsam diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index e3252a90b..84e521156 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -18,15 +18,10 @@ #include -#if 0 - #include -//#include #include #include -//#include #include -//#include #include #include #include @@ -34,8 +29,6 @@ #include #include #include -#include -#include #include #include #include @@ -49,6 +42,7 @@ #include #include #include +#include #include @@ -57,8 +51,6 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; typedef PriorFactor PriorFactorPoint2; typedef PriorFactor PriorFactorStereoPoint2; typedef PriorFactor PriorFactorPoint3; @@ -69,12 +61,9 @@ typedef PriorFactor PriorFactorPose3; typedef PriorFactor PriorFactorCal3_S2; typedef PriorFactor PriorFactorCal3DS2; typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; typedef PriorFactor PriorFactorPinholeCameraCal3_S2; typedef PriorFactor PriorFactorStereoCamera; -typedef BetweenFactor BetweenFactorLieVector; -typedef BetweenFactor BetweenFactorLieMatrix; typedef BetweenFactor BetweenFactorPoint2; typedef BetweenFactor BetweenFactorPoint3; typedef BetweenFactor BetweenFactorRot2; @@ -82,8 +71,6 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; typedef NonlinearEquality NonlinearEqualityPoint2; typedef NonlinearEquality NonlinearEqualityStereoPoint2; typedef NonlinearEquality NonlinearEqualityPoint3; @@ -94,7 +81,6 @@ typedef NonlinearEquality NonlinearEqualityPose3; typedef NonlinearEquality NonlinearEqualityCal3_S2; typedef NonlinearEquality NonlinearEqualityCal3DS2; typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; typedef NonlinearEquality NonlinearEqualityStereoCamera; @@ -148,8 +134,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -GTSAM_VALUE_EXPORT(gtsam::LieVector); -GTSAM_VALUE_EXPORT(gtsam::LieMatrix); GTSAM_VALUE_EXPORT(gtsam::Point2); GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); GTSAM_VALUE_EXPORT(gtsam::Point3); @@ -170,8 +154,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); @@ -182,11 +164,8 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); @@ -194,8 +173,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); @@ -206,7 +183,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3") BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); @@ -286,8 +262,6 @@ TEST (testSerializationSLAM, smallExample_nonlinear) { /* ************************************************************************* */ TEST (testSerializationSLAM, factors) { - LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished()); - LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0).finished()); Point2 point2(1.0, 2.0); StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); Point3 point3(1.0, 2.0, 3.0); @@ -311,8 +285,6 @@ TEST (testSerializationSLAM, factors) { b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15); Values values; - values.insert(a01, lieVector); - values.insert(a02, lieMatrix); values.insert(a03, point2); values.insert(a04, stereoPoint2); values.insert(a05, point3); @@ -344,8 +316,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsDereferencedXML(robust1)); EXPECT(equalsDereferencedBinary(robust1)); - PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4); - PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6); PriorFactorPoint2 priorFactorPoint2(a03, point2, model2); PriorFactorStereoPoint2 priorFactorStereoPoint2(a04, stereoPoint2, model3); PriorFactorPoint3 priorFactorPoint3(a05, point3, model3); @@ -356,11 +326,8 @@ TEST (testSerializationSLAM, factors) { PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5); PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9); PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6); - PriorFactorSimpleCamera priorFactorSimpleCamera(a13, simpleCamera, model11); PriorFactorStereoCamera priorFactorStereoCamera(a14, stereoCamera, model11); - BetweenFactorLieVector betweenFactorLieVector(a01, b01, lieVector, model4); - BetweenFactorLieMatrix betweenFactorLieMatrix(a02, b02, lieMatrix, model6); BetweenFactorPoint2 betweenFactorPoint2(a03, b03, point2, model2); BetweenFactorPoint3 betweenFactorPoint3(a05, b05, point3, model3); BetweenFactorRot2 betweenFactorRot2(a06, b06, rot2, model1); @@ -368,8 +335,6 @@ TEST (testSerializationSLAM, factors) { BetweenFactorPose2 betweenFactorPose2(a08, b08, pose2, model3); BetweenFactorPose3 betweenFactorPose3(a09, b09, pose3, model6); - NonlinearEqualityLieVector nonlinearEqualityLieVector(a01, lieVector); - NonlinearEqualityLieMatrix nonlinearEqualityLieMatrix(a02, lieMatrix); NonlinearEqualityPoint2 nonlinearEqualityPoint2(a03, point2); NonlinearEqualityStereoPoint2 nonlinearEqualityStereoPoint2(a04, stereoPoint2); NonlinearEqualityPoint3 nonlinearEqualityPoint3(a05, point3); @@ -380,7 +345,6 @@ TEST (testSerializationSLAM, factors) { NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2); NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2); NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera); - NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera); RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1); @@ -405,8 +369,6 @@ TEST (testSerializationSLAM, factors) { NonlinearFactorGraph graph; - graph += priorFactorLieVector; - graph += priorFactorLieMatrix; graph += priorFactorPoint2; graph += priorFactorStereoPoint2; graph += priorFactorPoint3; @@ -417,11 +379,8 @@ TEST (testSerializationSLAM, factors) { graph += priorFactorCal3_S2; graph += priorFactorCal3DS2; graph += priorFactorCalibratedCamera; - graph += priorFactorSimpleCamera; graph += priorFactorStereoCamera; - graph += betweenFactorLieVector; - graph += betweenFactorLieMatrix; graph += betweenFactorPoint2; graph += betweenFactorPoint3; graph += betweenFactorRot2; @@ -429,8 +388,6 @@ TEST (testSerializationSLAM, factors) { graph += betweenFactorPose2; graph += betweenFactorPose3; - graph += nonlinearEqualityLieVector; - graph += nonlinearEqualityLieMatrix; graph += nonlinearEqualityPoint2; graph += nonlinearEqualityStereoPoint2; graph += nonlinearEqualityPoint3; @@ -441,7 +398,6 @@ TEST (testSerializationSLAM, factors) { graph += nonlinearEqualityCal3_S2; graph += nonlinearEqualityCal3DS2; graph += nonlinearEqualityCalibratedCamera; - graph += nonlinearEqualitySimpleCamera; graph += nonlinearEqualityStereoCamera; graph += rangeFactor2D; @@ -471,8 +427,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(values)); EXPECT(equalsObj(graph)); - EXPECT(equalsObj(priorFactorLieVector)); - EXPECT(equalsObj(priorFactorLieMatrix)); EXPECT(equalsObj(priorFactorPoint2)); EXPECT(equalsObj(priorFactorStereoPoint2)); EXPECT(equalsObj(priorFactorPoint3)); @@ -483,11 +437,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(priorFactorCal3_S2)); EXPECT(equalsObj(priorFactorCal3DS2)); EXPECT(equalsObj(priorFactorCalibratedCamera)); - EXPECT(equalsObj(priorFactorSimpleCamera)); EXPECT(equalsObj(priorFactorStereoCamera)); - EXPECT(equalsObj(betweenFactorLieVector)); - EXPECT(equalsObj(betweenFactorLieMatrix)); EXPECT(equalsObj(betweenFactorPoint2)); EXPECT(equalsObj(betweenFactorPoint3)); EXPECT(equalsObj(betweenFactorRot2)); @@ -495,8 +446,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(betweenFactorPose2)); EXPECT(equalsObj(betweenFactorPose3)); - EXPECT(equalsObj(nonlinearEqualityLieVector)); - EXPECT(equalsObj(nonlinearEqualityLieMatrix)); EXPECT(equalsObj(nonlinearEqualityPoint2)); EXPECT(equalsObj(nonlinearEqualityStereoPoint2)); EXPECT(equalsObj(nonlinearEqualityPoint3)); @@ -507,7 +456,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(nonlinearEqualityCal3_S2)); EXPECT(equalsObj(nonlinearEqualityCal3DS2)); EXPECT(equalsObj(nonlinearEqualityCalibratedCamera)); - EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); EXPECT(equalsObj(nonlinearEqualityStereoCamera)); EXPECT(equalsObj(rangeFactor2D)); @@ -537,8 +485,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(values)); EXPECT(equalsXML(graph)); - EXPECT(equalsXML(priorFactorLieVector)); - EXPECT(equalsXML(priorFactorLieMatrix)); EXPECT(equalsXML(priorFactorPoint2)); EXPECT(equalsXML(priorFactorStereoPoint2)); EXPECT(equalsXML(priorFactorPoint3)); @@ -549,11 +495,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(priorFactorCal3_S2)); EXPECT(equalsXML(priorFactorCal3DS2)); EXPECT(equalsXML(priorFactorCalibratedCamera)); - EXPECT(equalsXML(priorFactorSimpleCamera)); EXPECT(equalsXML(priorFactorStereoCamera)); - EXPECT(equalsXML(betweenFactorLieVector)); - EXPECT(equalsXML(betweenFactorLieMatrix)); EXPECT(equalsXML(betweenFactorPoint2)); EXPECT(equalsXML(betweenFactorPoint3)); EXPECT(equalsXML(betweenFactorRot2)); @@ -561,8 +504,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(betweenFactorPose2)); EXPECT(equalsXML(betweenFactorPose3)); - EXPECT(equalsXML(nonlinearEqualityLieVector)); - EXPECT(equalsXML(nonlinearEqualityLieMatrix)); EXPECT(equalsXML(nonlinearEqualityPoint2)); EXPECT(equalsXML(nonlinearEqualityStereoPoint2)); EXPECT(equalsXML(nonlinearEqualityPoint3)); @@ -573,7 +514,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(nonlinearEqualityCal3_S2)); EXPECT(equalsXML(nonlinearEqualityCal3DS2)); EXPECT(equalsXML(nonlinearEqualityCalibratedCamera)); - EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); EXPECT(equalsXML(nonlinearEqualityStereoCamera)); EXPECT(equalsXML(rangeFactor2D)); @@ -603,8 +543,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(values)); EXPECT(equalsBinary(graph)); - EXPECT(equalsBinary(priorFactorLieVector)); - EXPECT(equalsBinary(priorFactorLieMatrix)); EXPECT(equalsBinary(priorFactorPoint2)); EXPECT(equalsBinary(priorFactorStereoPoint2)); EXPECT(equalsBinary(priorFactorPoint3)); @@ -615,11 +553,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(priorFactorCal3_S2)); EXPECT(equalsBinary(priorFactorCal3DS2)); EXPECT(equalsBinary(priorFactorCalibratedCamera)); - EXPECT(equalsBinary(priorFactorSimpleCamera)); EXPECT(equalsBinary(priorFactorStereoCamera)); - EXPECT(equalsBinary(betweenFactorLieVector)); - EXPECT(equalsBinary(betweenFactorLieMatrix)); EXPECT(equalsBinary(betweenFactorPoint2)); EXPECT(equalsBinary(betweenFactorPoint3)); EXPECT(equalsBinary(betweenFactorRot2)); @@ -627,8 +562,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(betweenFactorPose2)); EXPECT(equalsBinary(betweenFactorPose3)); - EXPECT(equalsBinary(nonlinearEqualityLieVector)); - EXPECT(equalsBinary(nonlinearEqualityLieMatrix)); EXPECT(equalsBinary(nonlinearEqualityPoint2)); EXPECT(equalsBinary(nonlinearEqualityStereoPoint2)); EXPECT(equalsBinary(nonlinearEqualityPoint3)); @@ -639,7 +572,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(nonlinearEqualityCal3_S2)); EXPECT(equalsBinary(nonlinearEqualityCal3DS2)); EXPECT(equalsBinary(nonlinearEqualityCalibratedCamera)); - EXPECT(equalsBinary(nonlinearEqualitySimpleCamera)); EXPECT(equalsBinary(nonlinearEqualityStereoCamera)); EXPECT(equalsBinary(rangeFactor2D)); @@ -663,7 +595,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(genericStereoFactor3D)); } -#endif /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From 8a5097d726764674f4df38ffc953aad51210c967 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sun, 26 Jul 2020 08:22:40 +0200 Subject: [PATCH 18/32] Add docs on serializing expressions. --- gtsam/nonlinear/ExpressionFactor.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index c42b2bdfc..e39a65a0e 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -27,8 +27,13 @@ namespace gtsam { /** - - * Factor that supports arbitrary expressions via AD + * Factor that supports arbitrary expressions via AD. + * + * Arbitrary instances of this template can be directly inserted into a factor + * graph for optimization. However, to enable the correct (de)serialization of + * such instances, the user should declare derived classes from this template, + * implementing expresion(), serialize(), clone(), print(), and defining the + * corresponding `struct traits : public Testable {}`. */ template class ExpressionFactor: public NoiseModelFactor { From 4e3638f6a72cfbb384d9c240e1aa57b71ad5b838 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sun, 26 Jul 2020 08:41:57 +0200 Subject: [PATCH 19/32] enable compiler warnings and errors for safer code --- cmake/GtsamBuildTypes.cmake | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 088ba7ad2..a233e562c 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -104,8 +104,17 @@ if(MSVC) set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") else() # Common to all configurations, next for each configuration: - # "-fPIC" is to ensure proper code generation for shared libraries - set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall -fPIC CACHE STRING "(User editable) Private compiler flags for all configurations.") + + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON + -Wall # Enable common warnings + -fPIC # ensure proper code generation for shared libraries + $<$:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address + $<$:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address + -Wreturn-type -Werror=return-type # Error on missing return() + -Wformat -Werror=format-security # Error on wrong printf() arguments + -Wsuggest-override -Werror=suggest-override # Enforce the use of the override keyword + # + CACHE STRING "(User editable) Private compiler flags for all configurations.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO -g -O3 CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE -O3 CACHE STRING "(User editable) Private compiler flags for Release configuration.") From 0198c648e3f5b162717704658ab9d28ccdbe5563 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sun, 26 Jul 2020 09:57:54 +0200 Subject: [PATCH 20/32] Fix all new gcc warnings/errors: make explicit virtual/override methods. Rules are: - use "virtual" in base classes only. - use "override" in all derived classes. --- CppUnitLite/Test.h | 4 +- cmake/GtsamBuildTypes.cmake | 2 +- examples/CameraResectioning.cpp | 4 +- examples/LocalizationExample.cpp | 4 +- gtsam/base/GenericValue.h | 18 ++--- gtsam/base/ThreadsafeException.h | 10 +-- .../treeTraversal/parallelTraversalTasks.h | 4 +- gtsam/discrete/DecisionTree-inl.h | 48 +++++------ gtsam/discrete/DecisionTreeFactor.h | 12 +-- gtsam/discrete/DiscreteConditional.h | 6 +- gtsam/geometry/Cal3DS2.h | 4 +- gtsam/geometry/Cal3DS2_Base.h | 2 +- gtsam/geometry/Cal3Unified.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/PinholePose.h | 2 +- gtsam/geometry/PinholeSet.h | 2 +- gtsam/inference/BayesTree.h | 2 +- gtsam/inference/BayesTreeCliqueBase.h | 2 +- gtsam/inference/inferenceExceptions.h | 6 +- gtsam/linear/BinaryJacobianFactor.h | 2 +- gtsam/linear/ConjugateGradientSolver.h | 2 +- gtsam/linear/GaussianConditional.h | 4 +- gtsam/linear/GaussianDensity.h | 2 +- gtsam/linear/LossFunctions.h | 8 +- gtsam/linear/PCGSolver.h | 6 +- gtsam/linear/Preconditioner.h | 16 ++-- gtsam/linear/RegularHessianFactor.h | 8 +- gtsam/linear/RegularJacobianFactor.h | 10 +-- gtsam/linear/SubgraphSolver.h | 2 +- gtsam/linear/linearExceptions.cpp | 6 +- gtsam/linear/linearExceptions.h | 14 ++-- gtsam/navigation/AHRSFactor.h | 10 +-- gtsam/navigation/AttitudeFactor.h | 24 +++--- gtsam/navigation/CombinedImuFactor.h | 14 ++-- gtsam/navigation/GPSFactor.h | 20 ++--- gtsam/navigation/ImuFactor.h | 20 ++--- gtsam/navigation/MagFactor.h | 8 +- gtsam/navigation/PreintegrationParams.h | 4 +- gtsam/nonlinear/ExpressionFactor.h | 14 ++-- gtsam/nonlinear/FunctorizedFactor.h | 8 +- gtsam/nonlinear/ISAM2Clique.h | 2 +- gtsam/nonlinear/LinearContainerFactor.h | 12 +-- gtsam/nonlinear/NonlinearEquality.h | 26 +++--- gtsam/nonlinear/NonlinearFactor.h | 26 +++--- gtsam/nonlinear/PriorFactor.h | 10 +-- gtsam/nonlinear/Values.cpp | 8 +- gtsam/nonlinear/Values.h | 36 ++++----- gtsam/nonlinear/WhiteNoiseFactor.h | 10 +-- gtsam/nonlinear/internal/CallRecord.h | 30 +++---- gtsam/nonlinear/internal/ExpressionNode.h | 80 +++++++++---------- gtsam/nonlinear/nonlinearExceptions.h | 6 +- gtsam/sam/BearingFactor.h | 4 +- gtsam/sam/BearingRangeFactor.h | 8 +- gtsam/sam/RangeFactor.h | 12 +-- gtsam/slam/AntiFactor.h | 14 ++-- gtsam/slam/BetweenFactor.h | 10 +-- gtsam/slam/BoundingConstraint.h | 8 +- gtsam/slam/EssentialMatrixConstraint.h | 12 +-- gtsam/slam/EssentialMatrixFactor.h | 24 +++--- gtsam/slam/FrobeniusFactor.h | 8 +- gtsam/slam/GeneralSFMFactor.h | 18 ++--- gtsam/slam/OrientedPlane3Factor.h | 18 ++--- gtsam/slam/PoseRotationPrior.h | 8 +- gtsam/slam/PoseTranslationPrior.h | 8 +- gtsam/slam/ProjectionFactor.h | 8 +- gtsam/slam/ReferenceFrameFactor.h | 10 +-- gtsam/slam/RotateFactor.h | 16 ++-- gtsam/slam/SmartFactorBase.h | 6 +- gtsam/slam/SmartProjectionFactor.h | 10 +-- gtsam/slam/SmartProjectionPoseFactor.h | 8 +- gtsam/slam/StereoFactor.h | 8 +- gtsam/slam/TriangulationFactor.h | 10 +-- gtsam/slam/tests/testSmartFactorBase.cpp | 12 +-- gtsam/symbolic/SymbolicConditional.h | 2 +- gtsam_unstable/discrete/AllDiff.h | 18 ++--- gtsam_unstable/discrete/BinaryAllDiff.h | 20 ++--- gtsam_unstable/discrete/Domain.h | 21 +++-- gtsam_unstable/discrete/SingleValue.h | 21 +++-- gtsam_unstable/dynamics/FullIMUFactor.h | 10 +-- gtsam_unstable/dynamics/IMUFactor.h | 10 +-- gtsam_unstable/dynamics/Pendulum.h | 16 ++-- gtsam_unstable/dynamics/SimpleHelicopter.h | 8 +- gtsam_unstable/dynamics/VelocityConstraint.h | 8 +- gtsam_unstable/dynamics/VelocityConstraint3.h | 4 +- .../linear/InfeasibleInitialValues.h | 4 +- .../linear/InfeasibleOrUnboundedProblem.h | 4 +- gtsam_unstable/linear/LinearCost.h | 10 +-- gtsam_unstable/linear/LinearEquality.h | 10 +-- gtsam_unstable/linear/LinearInequality.h | 10 +-- gtsam_unstable/linear/QPSParserException.h | 4 +- .../nonlinear/BatchFixedLagSmoother.h | 4 +- .../nonlinear/ConcurrentBatchFilter.h | 14 ++-- .../nonlinear/ConcurrentBatchSmoother.h | 14 ++-- .../nonlinear/ConcurrentIncrementalFilter.h | 14 ++-- .../nonlinear/ConcurrentIncrementalSmoother.h | 14 ++-- gtsam_unstable/nonlinear/LinearizedFactor.h | 24 +++--- gtsam_unstable/slam/BetweenFactorEM.h | 8 +- gtsam_unstable/slam/BiasedGPSFactor.h | 6 +- gtsam_unstable/slam/DummyFactor.h | 12 +-- .../slam/EquivInertialNavFactor_GlobalVel.h | 6 +- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 2 +- .../slam/GaussMarkov1stOrderFactor.h | 6 +- .../slam/InertialNavFactor_GlobalVelocity.h | 6 +- gtsam_unstable/slam/InvDepthFactor3.h | 6 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 6 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 6 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 12 +-- gtsam_unstable/slam/MultiProjectionFactor.h | 8 +- gtsam_unstable/slam/PartialPriorFactor.h | 8 +- gtsam_unstable/slam/PoseBetweenFactor.h | 8 +- gtsam_unstable/slam/PosePriorFactor.h | 8 +- gtsam_unstable/slam/ProjectionFactorPPP.h | 8 +- gtsam_unstable/slam/ProjectionFactorPPPC.h | 8 +- gtsam_unstable/slam/RelativeElevationFactor.h | 8 +- gtsam_unstable/slam/SmartRangeFactor.h | 12 +-- .../slam/SmartStereoProjectionFactor.h | 14 ++-- .../slam/SmartStereoProjectionPoseFactor.h | 8 +- gtsam_unstable/slam/TSAMFactors.h | 6 +- .../slam/TransformBtwRobotsUnaryFactor.h | 12 +-- .../slam/TransformBtwRobotsUnaryFactorEM.h | 12 +-- tests/simulated2D.h | 12 +-- tests/simulated2DConstraints.h | 18 ++--- tests/simulated2DOriented.h | 4 +- tests/simulated3D.h | 4 +- tests/smallExample.h | 2 +- tests/testExtendedKalmanFilter.cpp | 24 +++--- tests/testNonlinearFactor.cpp | 14 ++-- tests/testNonlinearOptimizer.cpp | 4 +- wrap/Method.h | 8 +- wrap/ReturnType.h | 2 +- wrap/StaticMethod.h | 6 +- wrap/utilities.h | 12 +-- 132 files changed, 696 insertions(+), 698 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index b1fb0cf08..a898c83ef 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -64,7 +64,7 @@ protected: class testGroup##testName##Test : public Test \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ virtual ~testGroup##testName##Test () {};\ - void run (TestResult& result_);} \ + void run (TestResult& result_) override;} \ testGroup##testName##Instance; \ void testGroup##testName##Test::run (TestResult& result_) @@ -82,7 +82,7 @@ protected: class testGroup##testName##Test : public Test \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ virtual ~testGroup##testName##Test () {};\ - void run (TestResult& result_);} \ + void run (TestResult& result_) override;} \ testGroup##testName##Instance; \ void testGroup##testName##Test::run (TestResult& result_) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index a233e562c..bffe97904 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -112,7 +112,7 @@ else() $<$:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address -Wreturn-type -Werror=return-type # Error on missing return() -Wformat -Werror=format-security # Error on wrong printf() arguments - -Wsuggest-override -Werror=suggest-override # Enforce the use of the override keyword + $<$:-Wsuggest-override -Werror=suggest-override> # Enforce the use of the override keyword # CACHE STRING "(User editable) Private compiler flags for all configurations.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.") diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index b12418098..7ac2de8b1 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -46,8 +46,8 @@ public: } /// evaluate the error - virtual Vector evaluateError(const Pose3& pose, boost::optional H = - boost::none) const { + Vector evaluateError(const Pose3& pose, boost::optional H = + boost::none) const override { PinholeCamera camera(pose, *K_); return camera.project(P_, H, boost::none, boost::none) - p_; } diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index a28ead5fe..e6a0f6622 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -85,7 +85,7 @@ class UnaryFactor: public NoiseModelFactor1 { // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. Vector evaluateError(const Pose2& q, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { // The measurement function for a GPS-like measurement is simple: // error_x = pose.x - measurement.x // error_y = pose.y - measurement.y @@ -99,7 +99,7 @@ class UnaryFactor: public NoiseModelFactor1 { // The second is a 'clone' function that allows the factor to be copied. Under most // circumstances, the following code that employs the default copy constructor should // work fine. - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 2ac3eb80c..98425adde 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -70,7 +70,7 @@ public: } /// equals implementing generic Value interface - virtual bool equals_(const Value& p, double tol = 1e-9) const { + bool equals_(const Value& p, double tol = 1e-9) const override { // Cast the base class Value pointer to a templated generic class pointer const GenericValue& genericValue2 = static_cast(p); // Return the result of using the equals traits for the derived class @@ -83,7 +83,7 @@ public: } /// Virtual print function, uses traits - virtual void print(const std::string& str) const { + void print(const std::string& str) const override { std::cout << "(" << demangle(typeid(T).name()) << ") "; traits::Print(value_, str); } @@ -91,7 +91,7 @@ public: /** * Create a duplicate object returned as a pointer to the generic Value interface. */ - virtual Value* clone_() const { + Value* clone_() const override { GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in return ptr; } @@ -99,19 +99,19 @@ public: /** * Destroy and deallocate this object, only if it was originally allocated using clone_(). */ - virtual void deallocate_() const { + void deallocate_() const override { delete this; } /** * Clone this value (normal clone on the heap, delete with 'delete' operator) */ - virtual boost::shared_ptr clone() const { + boost::shared_ptr clone() const override { return boost::allocate_shared(Eigen::aligned_allocator(), *this); } /// Generic Value interface version of retract - virtual Value* retract_(const Vector& delta) const { + Value* retract_(const Vector& delta) const override { // Call retract on the derived class using the retract trait function const T retractResult = traits::Retract(GenericValue::value(), delta); @@ -122,7 +122,7 @@ public: } /// Generic Value interface version of localCoordinates - virtual Vector localCoordinates_(const Value& value2) const { + Vector localCoordinates_(const Value& value2) const override { // Cast the base class Value pointer to a templated generic class pointer const GenericValue& genericValue2 = static_cast&>(value2); @@ -142,12 +142,12 @@ public: } /// Return run-time dimensionality - virtual size_t dim() const { + size_t dim() const override { return traits::GetDimension(value_); } /// Assignment operator - virtual Value& operator=(const Value& rhs) { + Value& operator=(const Value& rhs) override { // Cast the base class Value pointer to a derived class pointer const GenericValue& derivedRhs = static_cast(rhs); diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index 0f7c6131d..744759f5b 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -71,12 +71,12 @@ protected: String(description.begin(), description.end())) { } - /// Default destructor doesn't have the throw() - virtual ~ThreadsafeException() throw () { + /// Default destructor doesn't have the noexcept + virtual ~ThreadsafeException() noexcept { } public: - virtual const char* what() const throw () { + const char* what() const noexcept override { return description_ ? description_->c_str() : ""; } }; @@ -113,8 +113,8 @@ public: class CholeskyFailed : public gtsam::ThreadsafeException { public: - CholeskyFailed() throw() {} - virtual ~CholeskyFailed() throw() {} + CholeskyFailed() noexcept {} + virtual ~CholeskyFailed() noexcept {} }; } // namespace gtsam diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index b52d44e2a..87d5b0d4c 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -57,7 +57,7 @@ namespace gtsam { makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() + tbb::task* execute() override { if(isPostOrderPhase) { @@ -144,7 +144,7 @@ namespace gtsam { roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold) {} - tbb::task* execute() + tbb::task* execute() override { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 2efd069cc..dd10500e6 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -66,42 +66,42 @@ namespace gtsam { } /// Leaf-Leaf equality - bool sameLeaf(const Leaf& q) const { + bool sameLeaf(const Leaf& q) const override { return constant_ == q.constant_; } /// polymorphic equality: is q is a leaf, could be - bool sameLeaf(const Node& q) const { + bool sameLeaf(const Node& q) const override { return (q.isLeaf() && q.sameLeaf(*this)); } /** equality up to tolerance */ - bool equals(const Node& q, double tol) const { + bool equals(const Node& q, double tol) const override { const Leaf* other = dynamic_cast (&q); if (!other) return false; return std::abs(double(this->constant_ - other->constant_)) < tol; } /** print */ - void print(const std::string& s) const { + void print(const std::string& s) const override { bool showZero = true; if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; } /** to graphviz file */ - void dot(std::ostream& os, bool showZero) const { + void dot(std::ostream& os, bool showZero) const override { if (showZero || constant_) os << "\"" << this->id() << "\" [label=\"" << boost::format("%4.2g") % constant_ << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, } /** evaluate */ - const Y& operator()(const Assignment& x) const { + const Y& operator()(const Assignment& x) const override { return constant_; } /** apply unary operator */ - NodePtr apply(const Unary& op) const { + NodePtr apply(const Unary& op) const override { NodePtr f(new Leaf(op(constant_))); return f; } @@ -111,27 +111,27 @@ namespace gtsam { // Simply calls apply on argument to call correct virtual method: // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below) // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice) - NodePtr apply_f_op_g(const Node& g, const Binary& op) const { + NodePtr apply_f_op_g(const Node& g, const Binary& op) const override { return g.apply_g_op_fL(*this, op); } // Applying binary operator to two leaves results in a leaf - NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { + NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override { NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL return h; } // If second argument is a Choice node, call it's apply with leaf as second - NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { + NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override { return fC.apply_fC_op_gL(*this, op); // operand order back to normal } /** choose a branch, create new memory ! */ - NodePtr choose(const L& label, size_t index) const { + NodePtr choose(const L& label, size_t index) const override { return NodePtr(new Leaf(constant())); } - bool isLeaf() const { return true; } + bool isLeaf() const override { return true; } }; // Leaf @@ -175,7 +175,7 @@ namespace gtsam { return f; } - bool isLeaf() const { return false; } + bool isLeaf() const override { return false; } /** Constructor, given choice label and mandatory expected branch count */ Choice(const L& label, size_t count) : @@ -236,7 +236,7 @@ namespace gtsam { } /** print (as a tree) */ - void print(const std::string& s) const { + void print(const std::string& s) const override { std::cout << s << " Choice("; // std::cout << this << ","; std::cout << label_ << ") " << std::endl; @@ -245,7 +245,7 @@ namespace gtsam { } /** output to graphviz (as a a graph) */ - void dot(std::ostream& os, bool showZero) const { + void dot(std::ostream& os, bool showZero) const override { os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_ << "\"]\n"; for (size_t i = 0; i < branches_.size(); i++) { @@ -266,17 +266,17 @@ namespace gtsam { } /// Choice-Leaf equality: always false - bool sameLeaf(const Leaf& q) const { + bool sameLeaf(const Leaf& q) const override { return false; } /// polymorphic equality: if q is a leaf, could be... - bool sameLeaf(const Node& q) const { + bool sameLeaf(const Node& q) const override { return (q.isLeaf() && q.sameLeaf(*this)); } /** equality up to tolerance */ - bool equals(const Node& q, double tol) const { + bool equals(const Node& q, double tol) const override { const Choice* other = dynamic_cast (&q); if (!other) return false; if (this->label_ != other->label_) return false; @@ -288,7 +288,7 @@ namespace gtsam { } /** evaluate */ - const Y& operator()(const Assignment& x) const { + const Y& operator()(const Assignment& x) const override { #ifndef NDEBUG typename Assignment::const_iterator it = x.find(label_); if (it == x.end()) { @@ -314,7 +314,7 @@ namespace gtsam { } /** apply unary operator */ - NodePtr apply(const Unary& op) const { + NodePtr apply(const Unary& op) const override { boost::shared_ptr r(new Choice(label_, *this, op)); return Unique(r); } @@ -324,12 +324,12 @@ namespace gtsam { // Simply calls apply on argument to call correct virtual method: // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf) // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below) - NodePtr apply_f_op_g(const Node& g, const Binary& op) const { + NodePtr apply_f_op_g(const Node& g, const Binary& op) const override { return g.apply_g_op_fC(*this, op); } // If second argument of binary op is Leaf node, recurse on branches - NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { + NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override { boost::shared_ptr h(new Choice(label(), nrChoices())); for(NodePtr branch: branches_) h->push_back(fL.apply_f_op_g(*branch, op)); @@ -337,7 +337,7 @@ namespace gtsam { } // If second argument of binary op is Choice, call constructor - NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { + NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override { boost::shared_ptr h(new Choice(fC, *this, op)); return Unique(h); } @@ -352,7 +352,7 @@ namespace gtsam { } /** choose a branch, recursively */ - NodePtr choose(const L& label, size_t index) const { + NodePtr choose(const L& label, size_t index) const override { if (label_ == label) return branches_[index]; // choose branch diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 54cce56be..d1696a281 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -69,23 +69,23 @@ namespace gtsam { /// @{ /// equality - bool equals(const DiscreteFactor& other, double tol = 1e-9) const; + bool equals(const DiscreteFactor& other, double tol = 1e-9) const override; // print - virtual void print(const std::string& s = "DecisionTreeFactor:\n", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + void print(const std::string& s = "DecisionTreeFactor:\n", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /// @} /// @name Standard Interface /// @{ /// Value is just look up in AlgebraicDecisonTree - virtual double operator()(const Values& values) const { + double operator()(const Values& values) const override { return Potentials::operator()(values); } /// multiply two factors - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { return apply(f, ADT::Ring::mul); } @@ -95,7 +95,7 @@ namespace gtsam { } /// Convert into a decisiontree - virtual DecisionTreeFactor toDecisionTreeFactor() const { + DecisionTreeFactor toDecisionTreeFactor() const override { return *this; } diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 225e6e1d3..8299fab2c 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -85,10 +85,10 @@ public: /// GTSAM-style print void print(const std::string& s = "Discrete Conditional: ", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /// GTSAM-style equals - bool equals(const DiscreteFactor& other, double tol = 1e-9) const; + bool equals(const DiscreteFactor& other, double tol = 1e-9) const override; /// @} /// @name Standard Interface @@ -102,7 +102,7 @@ public: } /// Evaluate, just look up in AlgebraicDecisonTree - virtual double operator()(const Values& values) const { + double operator()(const Values& values) const override { return Potentials::operator()(values); } diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 3e62f758d..7fd453d45 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -60,7 +60,7 @@ public: /// @{ /// print with optional string - virtual void print(const std::string& s = "") const ; + void print(const std::string& s = "") const override; /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; @@ -86,7 +86,7 @@ public: /// @{ /// @return a deep copy of this object - virtual boost::shared_ptr clone() const { + boost::shared_ptr clone() const override { return boost::shared_ptr(new Cal3DS2(*this)); } diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 4da5d1360..a0ece8bdb 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -69,7 +69,7 @@ public: /// @{ /// print with optional string - virtual void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const; /// assert equality up to a tolerance bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index ae75c3916..381405d20 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -75,7 +75,7 @@ public: /// @{ /// print with optional string - virtual void print(const std::string& s = "") const ; + void print(const std::string& s = "") const override; /// assert equality up to a tolerance bool equals(const Cal3Unified& K, double tol = 10e-9) const; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 9a80b937b..cf449ce8c 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -175,7 +175,7 @@ public: } /// return calibration - const Calibration& calibration() const { + const Calibration& calibration() const override { return K_; } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 79dbb9ce9..3bf96c08b 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -361,7 +361,7 @@ public: } /// return calibration - virtual const CALIBRATION& calibration() const { + const CALIBRATION& calibration() const override { return *K_; } diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index a2896ca8d..7e3dae56f 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -45,7 +45,7 @@ public: /// @{ /// print - virtual void print(const std::string& s = "") const { + void print(const std::string& s = "") const override { Base::print(s); } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 9d632ff06..10fc513ee 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -299,7 +299,7 @@ namespace gtsam { this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); } - void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const { + void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const override { clique->print(s + "stored clique", formatter); } }; diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 6266e961c..7aca84635 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -100,7 +100,7 @@ namespace gtsam { bool equals(const DERIVED& other, double tol = 1e-9) const; /** print this node */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h index edd0e0aa5..4409b16c7 100644 --- a/gtsam/inference/inferenceExceptions.h +++ b/gtsam/inference/inferenceExceptions.h @@ -28,9 +28,9 @@ namespace gtsam { * with an ordering that does not include all of the variables. */ class InconsistentEliminationRequested : public std::exception { public: - InconsistentEliminationRequested() throw() {} - virtual ~InconsistentEliminationRequested() throw() {} - virtual const char* what() const throw() { + InconsistentEliminationRequested() noexcept {} + virtual ~InconsistentEliminationRequested() noexcept {} + const char* what() const noexcept override { return "An inference algorithm was called with inconsistent arguments. The\n" "factor graph, ordering, or variable index were inconsistent with each\n" diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h index 343548613..8b53c34dd 100644 --- a/gtsam/linear/BinaryJacobianFactor.h +++ b/gtsam/linear/BinaryJacobianFactor.h @@ -49,7 +49,7 @@ struct BinaryJacobianFactor: JacobianFactor { // Fixed-size matrix update void updateHessian(const KeyVector& infoKeys, - SymmetricBlockMatrix* info) const { + SymmetricBlockMatrix* info) const override { gttic(updateHessian_BinaryJacobianFactor); // Whiten the factor if it has a noise model const SharedDiagonal& model = get_model(); diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 2596a7403..c92390491 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -80,7 +80,7 @@ public: void print() const { Base::print(); } - virtual void print(std::ostream &os) const; + void print(std::ostream &os) const override; static std::string blasTranslator(const BLASKernel k) ; static BLASKernel blasTranslator(const std::string &s) ; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 8b41a4def..fdbe511f8 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -88,10 +88,10 @@ namespace gtsam { /** print */ void print(const std::string& = "GaussianConditional", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /** equals function */ - bool equals(const GaussianFactor&cg, double tol = 1e-9) const; + bool equals(const GaussianFactor&cg, double tol = 1e-9) const override; /** Return a view of the upper-triangular R block of the conditional */ constABlock R() const { return Ab_.range(0, nrFrontals()); } diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 03ffe9326..71af704ab 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -57,7 +57,7 @@ namespace gtsam { /// print void print(const std::string& = "GaussianDensity", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 6a5dc5a26..a126b37db 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -134,10 +134,10 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} - double weight(double /*error*/) const { return 1.0; } - double loss(double distance) const { return 0.5 * distance * distance; } - void print(const std::string &s) const; - bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } + double weight(double /*error*/) const override { return 1.0; } + double loss(double distance) const override { return 0.5 * distance * distance; } + void print(const std::string &s) const override; + bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; } static shared_ptr Create(); private: diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 7752902ba..515f2eed2 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -41,7 +41,7 @@ public: PCGSolverParameters() { } - virtual void print(std::ostream &os) const; + void print(std::ostream &os) const override; /* interface to preconditioner parameters */ inline const PreconditionerParameters& preconditioner() const { @@ -77,9 +77,9 @@ public: using IterativeSolver::optimize; - virtual VectorValues optimize(const GaussianFactorGraph &gfg, + VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial); + const VectorValues &initial) override; }; diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 31901db3f..eceb19982 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -111,13 +111,13 @@ public: virtual ~DummyPreconditioner() {} /* Computation Interfaces for raw vector */ - virtual void solve(const Vector& y, Vector &x) const { x = y; } - virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } - virtual void build( + void solve(const Vector& y, Vector &x) const override { x = y; } + void transposeSolve(const Vector& y, Vector& x) const override { x = y; } + void build( const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map &lambda - ) {} + ) override {} }; /*******************************************************************************************/ @@ -135,13 +135,13 @@ public: virtual ~BlockJacobiPreconditioner() ; /* Computation Interfaces for raw vector */ - virtual void solve(const Vector& y, Vector &x) const; - virtual void transposeSolve(const Vector& y, Vector& x) const ; - virtual void build( + void solve(const Vector& y, Vector &x) const override; + void transposeSolve(const Vector& y, Vector& x) const override; + void build( const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map &lambda - ) ; + ) override; protected: diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index a24acfacd..707f519ca 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -109,8 +109,8 @@ private: public: /** y += alpha * A'*A*x */ - virtual void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const override { HessianFactor::multiplyHessianAdd(alpha, x, y); } @@ -182,7 +182,7 @@ public: } /** Return the diagonal of the Hessian for this factor (raw memory version) */ - virtual void hessianDiagonal(double* d) const { + void hessianDiagonal(double* d) const override { // Loop over all variables in the factor for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { @@ -193,7 +193,7 @@ public: } /// Add gradient at zero to d TODO: is it really the goal to add ?? - virtual void gradientAtZero(double* d) const { + void gradientAtZero(double* d) const override { // Loop over all variables in the factor for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h index 0312efe78..1c465da03 100644 --- a/gtsam/linear/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -70,8 +70,8 @@ public: using JacobianFactor::multiplyHessianAdd; /** y += alpha * A'*A*x */ - virtual void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const override { JacobianFactor::multiplyHessianAdd(alpha, x, y); } @@ -106,7 +106,7 @@ public: using GaussianFactor::hessianDiagonal; /// Raw memory access version of hessianDiagonal - void hessianDiagonal(double* d) const { + void hessianDiagonal(double* d) const override { // Loop over all variables in the factor for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal @@ -125,12 +125,12 @@ public: } /// Expose base class gradientAtZero - virtual VectorValues gradientAtZero() const { + VectorValues gradientAtZero() const override { return JacobianFactor::gradientAtZero(); } /// Raw memory access version of gradientAtZero - void gradientAtZero(double* d) const { + void gradientAtZero(double* d) const override { // Get vector b not weighted Vector b = getb(); diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 5ab1a8520..0f7eea06f 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -38,7 +38,7 @@ struct GTSAM_EXPORT SubgraphSolverParameters explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) : builderParams(p) {} void print() const { Base::print(); } - virtual void print(std::ostream &os) const { + void print(std::ostream &os) const override { Base::print(os); } }; diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index a4168af2d..ada3a298c 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -24,7 +24,7 @@ namespace gtsam { /* ************************************************************************* */ - const char* IndeterminantLinearSystemException::what() const throw() + const char* IndeterminantLinearSystemException::what() const noexcept { if(!description_) { description_ = String( @@ -43,7 +43,7 @@ more information."); } /* ************************************************************************* */ - const char* InvalidNoiseModel::what() const throw() { + const char* InvalidNoiseModel::what() const noexcept { if(description_.empty()) description_ = (boost::format( "A JacobianFactor was attempted to be constructed or modified to use a\n" @@ -54,7 +54,7 @@ more information."); } /* ************************************************************************* */ - const char* InvalidMatrixBlock::what() const throw() { + const char* InvalidMatrixBlock::what() const noexcept { if(description_.empty()) description_ = (boost::format( "A JacobianFactor was attempted to be constructed with a matrix block of\n" diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 32db27fc9..f0ad0be39 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -94,10 +94,10 @@ namespace gtsam { class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException { Key j_; public: - IndeterminantLinearSystemException(Key j) throw() : j_(j) {} - virtual ~IndeterminantLinearSystemException() throw() {} + IndeterminantLinearSystemException(Key j) noexcept : j_(j) {} + virtual ~IndeterminantLinearSystemException() noexcept {} Key nearbyVariable() const { return j_; } - virtual const char* what() const throw(); + const char* what() const noexcept override; }; /* ************************************************************************* */ @@ -110,9 +110,9 @@ namespace gtsam { InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) : factorDims(factorDims), noiseModelDims(noiseModelDims) {} - virtual ~InvalidNoiseModel() throw() {} + virtual ~InvalidNoiseModel() noexcept {} - virtual const char* what() const throw(); + const char* what() const noexcept override; private: mutable std::string description_; @@ -128,9 +128,9 @@ namespace gtsam { InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) : factorRows(factorRows), blockRows(blockRows) {} - virtual ~InvalidMatrixBlock() throw() {} + virtual ~InvalidMatrixBlock() noexcept {} - virtual const char* what() const throw(); + const char* what() const noexcept override; private: mutable std::string description_; diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 1418ab687..c3765b8cf 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -158,14 +158,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const; + gtsam::NonlinearFactor::shared_ptr clone() const override; /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /// equals - virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const; + bool equals(const NonlinearFactor&, double tol = 1e-9) const override; /// Access the preintegrated measurements. const PreintegratedAhrsMeasurements& preintegratedMeasurements() const { @@ -178,7 +178,7 @@ public: Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, const Vector3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = - boost::none) const; + boost::none) const override; /// predicted states from IMU /// TODO(frank): relationship with PIM predict ?? diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 5a0031caf..9a0a11cfb 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -108,21 +108,21 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /** vector of errors */ - virtual Vector evaluateError(const Rot3& nRb, // - boost::optional H = boost::none) const { + Vector evaluateError(const Rot3& nRb, // + boost::optional H = boost::none) const override { return attitudeError(nRb, H); } @@ -182,21 +182,21 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /** vector of errors */ - virtual Vector evaluateError(const Pose3& nTb, // - boost::optional H = boost::none) const { + Vector evaluateError(const Pose3& nTb, // + boost::optional H = boost::none) const override { Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a89568433..6181f3d0d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -87,8 +87,8 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); } - void print(const std::string& s="") const; - bool equals(const PreintegratedRotationParams& other, double tol) const; + void print(const std::string& s="") const override; + bool equals(const PreintegratedRotationParams& other, double tol) const override; void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } @@ -305,7 +305,7 @@ public: virtual ~CombinedImuFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const; + gtsam::NonlinearFactor::shared_ptr clone() const override; /** implement functions needed for Testable */ @@ -314,11 +314,11 @@ public: GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const CombinedImuFactor&); /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /// @} /** Access the preintegrated measurements. */ @@ -336,7 +336,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = - boost::none, boost::optional H6 = boost::none) const; + boost::none, boost::optional H6 = boost::none) const override; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated typename diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 2b5ea1f2f..e6d5b88a9 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -65,21 +65,21 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /// vector of errors Vector evaluateError(const Pose3& p, - boost::optional H = boost::none) const; + boost::optional H = boost::none) const override; inline const Point3 & measurementIn() const { return nT_; @@ -137,21 +137,21 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /// vector of errors Vector evaluateError(const NavState& p, - boost::optional H = boost::none) const; + boost::optional H = boost::none) const override; inline const Point3 & measurementIn() const { return nT_; diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 7e080ffd5..65142af24 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -217,14 +217,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const; + gtsam::NonlinearFactor::shared_ptr clone() const override; /// @name Testable /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&); - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /// @} /** Access the preintegrated measurements. */ @@ -241,7 +241,7 @@ public: const imuBias::ConstantBias& bias_i, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = - boost::none, boost::optional H5 = boost::none) const; + boost::none, boost::optional H5 = boost::none) const override; #ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge two pre-integrated measurement classes @@ -315,14 +315,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const; + gtsam::NonlinearFactor::shared_ptr clone() const override; /// @name Testable /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&); - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /// @} /** Access the preintegrated measurements. */ @@ -338,7 +338,7 @@ public: const imuBias::ConstantBias& bias_i, // boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const; + boost::optional H3 = boost::none) const override; private: diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 3875391d0..97a4c70ce 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -53,7 +53,7 @@ public: } /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor(*this))); } @@ -102,7 +102,7 @@ public: } /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor1(*this))); } @@ -138,7 +138,7 @@ public: } /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor2(*this))); } @@ -179,7 +179,7 @@ public: } /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor3(*this))); } diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 9ae66e678..ce1f0e734 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -56,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { return boost::shared_ptr(new PreintegrationParams(Vector3(0, 0, -g))); } - void print(const std::string& s="") const; - bool equals(const PreintegratedRotationParams& other, double tol) const; + void print(const std::string& s="") const override; + bool equals(const PreintegratedRotationParams& other, double tol) const override; void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index c42b2bdfc..2e78aafe2 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -68,13 +68,13 @@ protected: /// print relies on Testable traits being defined for T void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { NoiseModelFactor::print(s, keyFormatter); traits::Print(measured_, "ExpressionFactor with measurement: "); } /// equals relies on Testable traits being defined for T - bool equals(const NonlinearFactor& f, double tol) const { + bool equals(const NonlinearFactor& f, double tol) const override { const ExpressionFactor* p = dynamic_cast(&f); return p && NoiseModelFactor::equals(f, tol) && traits::Equals(measured_, p->measured_, tol) && @@ -86,8 +86,8 @@ protected: * We override this method to provide * both the function evaluation and its derivative(s) in H. */ - virtual Vector unwhitenedError(const Values& x, - boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const override { if (H) { const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here @@ -99,7 +99,7 @@ protected: } } - virtual boost::shared_ptr linearize(const Values& x) const { + boost::shared_ptr linearize(const Values& x) const override { // Only linearize if the factor is active if (!active(x)) return boost::shared_ptr(); @@ -138,7 +138,7 @@ protected: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -263,7 +263,7 @@ class ExpressionFactor2 : public ExpressionFactor { private: /// Return an expression that predicts the measurement given Values - virtual Expression expression() const { + Expression expression() const override { return expression(this->keys_[0], this->keys_[1]); } diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 5351b126f..688b3aaa2 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -82,13 +82,13 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { virtual ~FunctorizedFactor() {} /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new FunctorizedFactor(*this))); } Vector evaluateError(const T ¶ms, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { R x = func_(params, H); Vector error = traits::Local(measured_, x); return error; @@ -97,7 +97,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { /// @name Testable /// @{ void print(const std::string &s = "", - const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" << keyFormatter(this->key()) << ")" << std::endl; @@ -106,7 +106,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { << std::endl; } - virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const { + bool equals(const NonlinearFactor &other, double tol = 1e-9) const override { const FunctorizedFactor *e = dynamic_cast *>(&other); return e && Base::equals(other, tol) && diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index 53bdaec81..467741d66 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -85,7 +85,7 @@ class GTSAM_EXPORT ISAM2Clique /** print this node */ void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const override; void optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed, VectorValues* delta, diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 8a1f600ff..4540117b6 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -59,10 +59,10 @@ public: // Testable /** print */ - GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override; /** Check if two factors are equal */ - GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const override; // NonlinearFactor @@ -74,10 +74,10 @@ public: * * @return nonlinear error if linearizationPoint present, zero otherwise */ - GTSAM_EXPORT double error(const Values& c) const; + GTSAM_EXPORT double error(const Values& c) const override; /** get the dimension of the factor: rows of linear factor */ - GTSAM_EXPORT size_t dim() const; + GTSAM_EXPORT size_t dim() const override; /** Extract the linearization point used in recalculating error */ const boost::optional& linearizationPoint() const { return linearizationPoint_; } @@ -98,7 +98,7 @@ public: * TODO: better approximation of relinearization * TODO: switchable modes for approximation technique */ - GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const; + GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const override; /** * Creates an anti-factor directly @@ -116,7 +116,7 @@ public: * * Clones the underlying linear factor */ - NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_)); } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 1bba57051..6286b73da 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -107,15 +107,15 @@ public: /// @name Testable /// @{ - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; traits::Print(feasible_, "Feasible Point:\n"); std::cout << "Variable Dimension: " << traits::GetDimension(feasible_) << std::endl; } /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const This* e = dynamic_cast(&f); return e && Base::equals(f) && traits::Equals(feasible_,e->feasible_, tol) && std::abs(error_gain_ - e->error_gain_) < tol; @@ -126,7 +126,7 @@ public: /// @{ /** actual error function calculation */ - virtual double error(const Values& c) const { + double error(const Values& c) const override { const T& xj = c.at(this->key()); Vector e = this->unwhitenedError(c); if (allow_error_ || !compare_(xj, feasible_)) { @@ -138,7 +138,7 @@ public: /** error function */ Vector evaluateError(const T& xj, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) @@ -158,7 +158,7 @@ public: } // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactor::shared_ptr linearize(const Values& x) const { + GaussianFactor::shared_ptr linearize(const Values& x) const override { const T& xj = x.at(this->key()); Matrix A; Vector b = evaluateError(xj, A); @@ -168,7 +168,7 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -242,14 +242,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** g(x) with optional derivative */ Vector evaluateError(const X& x1, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) @@ -257,8 +257,8 @@ public: } /** Print */ - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key()) << ")," << "\n"; this->noiseModel_->print(); @@ -317,14 +317,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { + boost::none, boost::optional H2 = boost::none) const override { static const size_t p = traits::dimension; if (H1) *H1 = -Matrix::Identity(p,p); if (H2) *H2 = Matrix::Identity(p,p); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 63547a248..7bbd51236 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -31,7 +31,7 @@ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #define ADD_CLONE_NONLINEAR_FACTOR(Derived) \ - virtual gtsam::NonlinearFactor::shared_ptr clone() const { \ + gtsam::NonlinearFactor::shared_ptr clone() const override { \ return boost::static_pointer_cast( \ gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); } #endif @@ -195,14 +195,14 @@ protected: public: /** Print */ - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override; /** get the dimension of the factor (number of rows on linearization) */ - virtual size_t dim() const { + size_t dim() const override { return noiseModel_->dim(); } @@ -242,14 +242,14 @@ public: * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ - virtual double error(const Values& c) const; + double error(const Values& c) const override; /** * Linearize a non-linearFactorN to get a GaussianFactor, * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x) const; + boost::shared_ptr linearize(const Values& x) const override; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated @@ -315,7 +315,7 @@ public: /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { if(this->active(x)) { const X& x1 = x.at(keys_[0]); if(H) { @@ -389,7 +389,7 @@ public: /** Calls the 2-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { if(this->active(x)) { const X1& x1 = x.at(keys_[0]); const X2& x2 = x.at(keys_[1]); @@ -467,7 +467,7 @@ public: /** Calls the 3-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { if(this->active(x)) { if(H) return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); @@ -547,7 +547,7 @@ public: /** Calls the 4-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { if(this->active(x)) { if(H) return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); @@ -631,7 +631,7 @@ public: /** Calls the 5-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { if(this->active(x)) { if(H) return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); @@ -719,7 +719,7 @@ public: /** Calls the 6-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { if(this->active(x)) { if(H) return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 0afbaa588..4f7075f21 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -65,15 +65,15 @@ namespace gtsam { } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + void print(const std::string& s, + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; traits::Print(prior_, " prior mean: "); if (this->noiseModel_) @@ -83,7 +83,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This* e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); } @@ -91,7 +91,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& x, boost::optional H = boost::none) const { + Vector evaluateError(const T& x, boost::optional H = boost::none) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) // TODO(ASL) Add Jacobians. diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 98790ccd9..7e13a072a 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -214,7 +214,7 @@ namespace gtsam { } /* ************************************************************************* */ - const char* ValuesKeyAlreadyExists::what() const throw() { + const char* ValuesKeyAlreadyExists::what() const noexcept { if(message_.empty()) message_ = "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists."; @@ -222,7 +222,7 @@ namespace gtsam { } /* ************************************************************************* */ - const char* ValuesKeyDoesNotExist::what() const throw() { + const char* ValuesKeyDoesNotExist::what() const noexcept { if(message_.empty()) message_ = "Attempting to " + std::string(operation_) + " the key \"" + @@ -231,7 +231,7 @@ namespace gtsam { } /* ************************************************************************* */ - const char* ValuesIncorrectType::what() const throw() { + const char* ValuesIncorrectType::what() const noexcept { if(message_.empty()) { std::string storedTypeName = demangle(storedTypeId_.name()); std::string requestedTypeName = demangle(requestedTypeId_.name()); @@ -251,7 +251,7 @@ namespace gtsam { } /* ************************************************************************* */ - const char* NoMatchFoundForFixed::what() const throw() { + const char* NoMatchFoundForFixed::what() const noexcept { if(message_.empty()) { ostringstream oss; oss diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 041aa3441..bc64f2612 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -432,16 +432,16 @@ namespace gtsam { public: /// Construct with the key-value pair attempted to be added - ValuesKeyAlreadyExists(Key key) throw() : + ValuesKeyAlreadyExists(Key key) noexcept : key_(key) {} - virtual ~ValuesKeyAlreadyExists() throw() {} + virtual ~ValuesKeyAlreadyExists() noexcept {} /// The duplicate key that was attempted to be added - Key key() const throw() { return key_; } + Key key() const noexcept { return key_; } /// The message to be displayed to the user - GTSAM_EXPORT virtual const char* what() const throw(); + GTSAM_EXPORT const char* what() const noexcept override; }; /* ************************************************************************* */ @@ -455,16 +455,16 @@ namespace gtsam { public: /// Construct with the key that does not exist in the values - ValuesKeyDoesNotExist(const char* operation, Key key) throw() : + ValuesKeyDoesNotExist(const char* operation, Key key) noexcept : operation_(operation), key_(key) {} - virtual ~ValuesKeyDoesNotExist() throw() {} + virtual ~ValuesKeyDoesNotExist() noexcept {} /// The key that was attempted to be accessed that does not exist - Key key() const throw() { return key_; } + Key key() const noexcept { return key_; } /// The message to be displayed to the user - GTSAM_EXPORT virtual const char* what() const throw(); + GTSAM_EXPORT const char* what() const noexcept override; }; /* ************************************************************************* */ @@ -480,13 +480,13 @@ namespace gtsam { public: /// Construct with the key that does not exist in the values ValuesIncorrectType(Key key, - const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() : + const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept : key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {} - virtual ~ValuesIncorrectType() throw() {} + virtual ~ValuesIncorrectType() noexcept {} /// The key that was attempted to be accessed that does not exist - Key key() const throw() { return key_; } + Key key() const noexcept { return key_; } /// The typeid of the value stores in the Values const std::type_info& storedTypeId() const { return storedTypeId_; } @@ -495,18 +495,18 @@ namespace gtsam { const std::type_info& requestedTypeId() const { return requestedTypeId_; } /// The message to be displayed to the user - GTSAM_EXPORT virtual const char* what() const throw(); + GTSAM_EXPORT const char* what() const noexcept override; }; /* ************************************************************************* */ class DynamicValuesMismatched : public std::exception { public: - DynamicValuesMismatched() throw() {} + DynamicValuesMismatched() noexcept {} - virtual ~DynamicValuesMismatched() throw() {} + virtual ~DynamicValuesMismatched() noexcept {} - virtual const char* what() const throw() { + const char* what() const noexcept override { return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values"; } }; @@ -522,14 +522,14 @@ namespace gtsam { mutable std::string message_; public: - NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () : + NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept : M1_(M1), N1_(N1), M2_(M2), N2_(N2) { } - virtual ~NoMatchFoundForFixed() throw () { + virtual ~NoMatchFoundForFixed() noexcept { } - GTSAM_EXPORT virtual const char* what() const throw (); + GTSAM_EXPORT const char* what() const noexcept override; }; /* ************************************************************************* */ diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 634736800..974998830 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -109,7 +109,7 @@ namespace gtsam { /// Print void print(const std::string& p = "WhiteNoiseFactor", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(p, keyFormatter); std::cout << p + ".z: " << z_ << std::endl; } @@ -119,12 +119,12 @@ namespace gtsam { /// @{ /// get the dimension of the factor (number of rows on linearization) - virtual size_t dim() const { + size_t dim() const override { return 2; } /// Calculate the error of the factor, typically equal to log-likelihood - inline double error(const Values& x) const { + double error(const Values& x) const override { return f(z_, x.at(meanKey_), x.at(precisionKey_)); } @@ -153,7 +153,7 @@ namespace gtsam { /// @{ /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& x) const { + boost::shared_ptr linearize(const Values& x) const override { double u = x.at(meanKey_); double p = x.at(precisionKey_); Key j1 = meanKey_; @@ -163,7 +163,7 @@ namespace gtsam { // TODO: Frank commented this out for now, can it go? // /// @return a deep copy of this factor - // virtual gtsam::NonlinearFactor::shared_ptr clone() const { + // gtsam::NonlinearFactor::shared_ptr clone() const override { // return boost::static_pointer_cast( // gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index 8bdc0652e..707c617ee 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -144,43 +144,43 @@ private: return static_cast(*this); } - virtual void _print(const std::string& indent) const { + void _print(const std::string& indent) const override { derived().print(indent); } // Called from base class non-virtual inline method startReverseAD2 // Calls non-virtual function startReverseAD4, implemented in Derived (ExpressionNode::Record) - virtual void _startReverseAD3(JacobianMap& jacobians) const { + void _startReverseAD3(JacobianMap& jacobians) const override { derived().startReverseAD4(jacobians); } - virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const { + void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const override { derived().reverseAD4(dFdT, jacobians); } - virtual void _reverseAD3( + void _reverseAD3( const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { + JacobianMap& jacobians) const override { derived().reverseAD4(dFdT, jacobians); } - virtual void _reverseAD3(const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { + void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const override { derived().reverseAD4(dFdT, jacobians); } - virtual void _reverseAD3(const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { + void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const override { derived().reverseAD4(dFdT, jacobians); } - virtual void _reverseAD3(const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { + void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const override { derived().reverseAD4(dFdT, jacobians); } - virtual void _reverseAD3(const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { + void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const override { derived().reverseAD4(dFdT, jacobians); } - virtual void _reverseAD3(const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { + void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const override { derived().reverseAD4(dFdT, jacobians); } }; diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 0ae37f130..ee3dc8929 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -135,18 +135,18 @@ public: } /// Print - virtual void print(const std::string& indent = "") const { + void print(const std::string& indent = "") const override { std::cout << indent << "Constant" << std::endl; } /// Return value - virtual T value(const Values& values) const { + T value(const Values& values) const override { return constant_; } /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { + T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const override { return constant_; } @@ -176,30 +176,30 @@ public: } /// Print - virtual void print(const std::string& indent = "") const { + void print(const std::string& indent = "") const override { std::cout << indent << "Leaf, key = " << DefaultKeyFormatter(key_) << std::endl; } /// Return keys that play in this expression - virtual std::set keys() const { + std::set keys() const override { std::set keys; keys.insert(key_); return keys; } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + void dims(std::map& map) const override { map[key_] = traits::dimension; } /// Return value - virtual T value(const Values& values) const { + T value(const Values& values) const override { return values.at(key_); } /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { + T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const override { trace.setLeaf(key_); return values.at(key_); } @@ -248,23 +248,23 @@ public: } /// Print - virtual void print(const std::string& indent = "") const { + void print(const std::string& indent = "") const override { std::cout << indent << "UnaryExpression" << std::endl; expression1_->print(indent + " "); } /// Return value - virtual T value(const Values& values) const { + T value(const Values& values) const override { return function_(expression1_->value(values), boost::none); } /// Return keys that play in this expression - virtual std::set keys() const { + std::set keys() const override { return expression1_->keys(); } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + void dims(std::map& map) const override { expression1_->dims(map); } @@ -307,8 +307,8 @@ public: }; /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* ptr) const { + T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const override { assert(reinterpret_cast(ptr) % TraceAlignment == 0); // Create a Record in the memory pointed to by ptr @@ -357,21 +357,21 @@ public: } /// Print - virtual void print(const std::string& indent = "") const { + void print(const std::string& indent = "") const override { std::cout << indent << "BinaryExpression" << std::endl; expression1_->print(indent + " "); expression2_->print(indent + " "); } /// Return value - virtual T value(const Values& values) const { + T value(const Values& values) const override { using boost::none; return function_(expression1_->value(values), expression2_->value(values), none, none); } /// Return keys that play in this expression - virtual std::set keys() const { + std::set keys() const override { std::set keys = expression1_->keys(); std::set myKeys = expression2_->keys(); keys.insert(myKeys.begin(), myKeys.end()); @@ -379,7 +379,7 @@ public: } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + void dims(std::map& map) const override { expression1_->dims(map); expression2_->dims(map); } @@ -426,8 +426,8 @@ public: }; /// Construct an execution trace for reverse AD, see UnaryExpression for explanation - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* ptr) const { + T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const override { assert(reinterpret_cast(ptr) % TraceAlignment == 0); Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr); trace.setFunction(record); @@ -464,7 +464,7 @@ public: } /// Print - virtual void print(const std::string& indent = "") const { + void print(const std::string& indent = "") const override { std::cout << indent << "TernaryExpression" << std::endl; expression1_->print(indent + " "); expression2_->print(indent + " "); @@ -472,14 +472,14 @@ public: } /// Return value - virtual T value(const Values& values) const { + T value(const Values& values) const override { using boost::none; return function_(expression1_->value(values), expression2_->value(values), expression3_->value(values), none, none, none); } /// Return keys that play in this expression - virtual std::set keys() const { + std::set keys() const override { std::set keys = expression1_->keys(); std::set myKeys = expression2_->keys(); keys.insert(myKeys.begin(), myKeys.end()); @@ -489,7 +489,7 @@ public: } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + void dims(std::map& map) const override { expression1_->dims(map); expression2_->dims(map); expression3_->dims(map); @@ -544,8 +544,8 @@ public: }; /// Construct an execution trace for reverse AD, see UnaryExpression for explanation - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* ptr) const { + T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const override { assert(reinterpret_cast(ptr) % TraceAlignment == 0); Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr); trace.setFunction(record); @@ -574,23 +574,23 @@ class ScalarMultiplyNode : public ExpressionNode { virtual ~ScalarMultiplyNode() {} /// Print - virtual void print(const std::string& indent = "") const { + void print(const std::string& indent = "") const override { std::cout << indent << "ScalarMultiplyNode" << std::endl; expression_->print(indent + " "); } /// Return value - virtual T value(const Values& values) const { + T value(const Values& values) const override { return scalar_ * expression_->value(values); } /// Return keys that play in this expression - virtual std::set keys() const { + std::set keys() const override { return expression_->keys(); } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + void dims(std::map& map) const override { expression_->dims(map); } @@ -624,8 +624,8 @@ class ScalarMultiplyNode : public ExpressionNode { }; /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* ptr) const { + T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const override { assert(reinterpret_cast(ptr) % TraceAlignment == 0); Record* record = new (ptr) Record(); ptr += upAligned(sizeof(Record)); @@ -662,19 +662,19 @@ class BinarySumNode : public ExpressionNode { virtual ~BinarySumNode() {} /// Print - virtual void print(const std::string& indent = "") const { + void print(const std::string& indent = "") const override { std::cout << indent << "BinarySumNode" << std::endl; expression1_->print(indent + " "); expression2_->print(indent + " "); } /// Return value - virtual T value(const Values& values) const { + T value(const Values& values) const override { return expression1_->value(values) + expression2_->value(values); } /// Return keys that play in this expression - virtual std::set keys() const { + std::set keys() const override { std::set keys = expression1_->keys(); std::set myKeys = expression2_->keys(); keys.insert(myKeys.begin(), myKeys.end()); @@ -682,7 +682,7 @@ class BinarySumNode : public ExpressionNode { } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + void dims(std::map& map) const override { expression1_->dims(map); expression2_->dims(map); } @@ -717,8 +717,8 @@ class BinarySumNode : public ExpressionNode { }; /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* ptr) const { + T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const override { assert(reinterpret_cast(ptr) % TraceAlignment == 0); Record* record = new (ptr) Record(); trace.setFunction(record); diff --git a/gtsam/nonlinear/nonlinearExceptions.h b/gtsam/nonlinear/nonlinearExceptions.h index 8b32b6fdc..7b704ac39 100644 --- a/gtsam/nonlinear/nonlinearExceptions.h +++ b/gtsam/nonlinear/nonlinearExceptions.h @@ -35,11 +35,11 @@ namespace gtsam { KeyFormatter formatter_; mutable std::string what_; public: - MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) throw() : + MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) noexcept : key_(key), formatter_(formatter) {} - virtual ~MarginalizeNonleafException() throw() {} + virtual ~MarginalizeNonleafException() noexcept {} Key key() const { return key_; } - virtual const char* what() const throw() { + const char* what() const noexcept override { if(what_.empty()) what_ = "\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\ diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index a9ed5ef4b..530fcfdcd 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -48,7 +48,7 @@ struct BearingFactor : public ExpressionFactor2 { } // Return measurement expression - virtual Expression expression(Key key1, Key key2) const { + Expression expression(Key key1, Key key2) const override { Expression a1_(key1); Expression a2_(key2); return Expression(Bearing(), a1_, a2_); @@ -56,7 +56,7 @@ struct BearingFactor : public ExpressionFactor2 { /// print void print(const std::string& s = "", - const KeyFormatter& kf = DefaultKeyFormatter) const { + const KeyFormatter& kf = DefaultKeyFormatter) const override { std::cout << s << "BearingFactor" << std::endl; Base::print(s, kf); } diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 44740f8ff..be645ef94 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -55,20 +55,20 @@ class BearingRangeFactor virtual ~BearingRangeFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // Return measurement expression - virtual Expression expression(Key key1, Key key2) const { + Expression expression(Key key1, Key key2) const override { return Expression(T::Measure, Expression(key1), Expression(key2)); } /// print - virtual void print(const std::string& s = "", - const KeyFormatter& kf = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& kf = DefaultKeyFormatter) const override { std::cout << s << "BearingRangeFactor" << std::endl; Base::print(s, kf); } diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 40a9cf758..80b02404e 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -47,13 +47,13 @@ class RangeFactor : public ExpressionFactor2 { } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // Return measurement expression - virtual Expression expression(Key key1, Key key2) const { + Expression expression(Key key1, Key key2) const override { Expression a1_(key1); Expression a2_(key2); return Expression(Range(), a1_, a2_); @@ -61,7 +61,7 @@ class RangeFactor : public ExpressionFactor2 { /// print void print(const std::string& s = "", - const KeyFormatter& kf = DefaultKeyFormatter) const { + const KeyFormatter& kf = DefaultKeyFormatter) const override { std::cout << s << "RangeFactor" << std::endl; Base::print(s, kf); } @@ -107,13 +107,13 @@ class RangeFactorWithTransform : public ExpressionFactor2 { virtual ~RangeFactorWithTransform() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // Return measurement expression - virtual Expression expression(Key key1, Key key2) const { + Expression expression(Key key1, Key key2) const override { Expression body_T_sensor__(body_T_sensor_); Expression nav_T_body_(key1); Expression nav_T_sensor_(traits::Compose, nav_T_body_, @@ -124,7 +124,7 @@ class RangeFactorWithTransform : public ExpressionFactor2 { /** print contents */ void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "RangeFactorWithTransform" << std::endl; this->body_T_sensor_.print(" sensor pose in body frame: "); Base::print(s, keyFormatter); diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index 9e4f7db5a..277080b6b 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -52,20 +52,20 @@ namespace gtsam { virtual ~AntiFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "AntiFactor version of:" << std::endl; factor_->print(s, keyFormatter); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); } @@ -77,16 +77,16 @@ namespace gtsam { * For the AntiFactor, this will have the same magnitude of the original factor, * but the opposite sign. */ - double error(const Values& c) const { return -factor_->error(c); } + double error(const Values& c) const override { return -factor_->error(c); } /** get the dimension of the factor (same as the original factor) */ - size_t dim() const { return factor_->dim(); } + size_t dim() const override { return factor_->dim(); } /** * Checks whether this factor should be used based on a set of values. * The AntiFactor will have the same 'active' profile as the original factor. */ - bool active(const Values& c) const { return factor_->active(c); } + bool active(const Values& c) const override { return factor_->active(c); } /** * Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor @@ -94,7 +94,7 @@ namespace gtsam { * returns a Jacobian instead of a full Hessian), but with the opposite sign. This * effectively cancels the effect of the original factor on the factor graph. */ - boost::shared_ptr linearize(const Values& c) const { + boost::shared_ptr linearize(const Values& c) const override { // Generate the linearized factor from the contained nonlinear factor GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index b1d4904aa..0f5aa6a4c 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -63,14 +63,14 @@ namespace gtsam { virtual ~BetweenFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; @@ -79,7 +79,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } @@ -87,8 +87,8 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { + Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const override { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index f6561807e..43cc6d292 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -58,14 +58,14 @@ struct BoundingConstraint1: public NoiseModelFactor1 { boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const Values& c) const { + bool active(const Values& c) const override { // note: still active at equality to avoid zigzagging double x = value(c.at(this->key())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } Vector evaluateError(const X& x, boost::optional H = - boost::none) const { + boost::none) const override { Matrix D; double error = value(x, D) - threshold_; if (H) { @@ -126,7 +126,7 @@ struct BoundingConstraint2: public NoiseModelFactor2 { boost::optional H2 = boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const Values& c) const { + bool active(const Values& c) const override { // note: still active at equality to avoid zigzagging double x = value(c.at(this->key1()), c.at(this->key2())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; @@ -134,7 +134,7 @@ struct BoundingConstraint2: public NoiseModelFactor2 { Vector evaluateError(const X1& x1, const X2& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { Matrix D1, D2; double error = value(x1, x2, D1, D2) - threshold_; if (H1) { diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index e474ce5b3..943db7207 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -61,7 +61,7 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -69,18 +69,18 @@ public: /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /** implement functions needed to derive from Factor */ /** vector of errors */ - virtual Vector evaluateError(const Pose3& p1, const Pose3& p2, + Vector evaluateError(const Pose3& p1, const Pose3& p2, boost::optional Hp1 = boost::none, // - boost::optional Hp2 = boost::none) const; + boost::optional Hp2 = boost::none) const override; /** return the measured */ const EssentialMatrix& measured() const { diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index c214a2f48..a99ac9512 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -58,14 +58,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor with measurements\n (" << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" @@ -74,7 +74,7 @@ public: /// vector of errors returns 1D vector Vector evaluateError(const EssentialMatrix& E, boost::optional H = - boost::none) const { + boost::none) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; @@ -131,14 +131,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" << dP1_.transpose() << ")' and (" << pn_.transpose() @@ -152,7 +152,7 @@ public: */ Vector evaluateError(const EssentialMatrix& E, const double& d, boost::optional DE = boost::none, boost::optional Dd = - boost::none) const { + boost::none) const override { // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d @@ -250,14 +250,14 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; } @@ -269,7 +269,7 @@ public: */ Vector evaluateError(const EssentialMatrix& E, const double& d, boost::optional DE = boost::none, boost::optional Dd = - boost::none) const { + boost::none) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index a73445ae0..6b2ef67fc 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -56,7 +56,7 @@ class FrobeniusPrior : public NoiseModelFactor1 { /// Error is just Frobenius norm between Rot element and vectorized matrix M. Vector evaluateError(const Rot& R, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { return R.vec(H) - vecM_; // Jacobian is computed only when needed. } }; @@ -78,7 +78,7 @@ class FrobeniusFactor : public NoiseModelFactor2 { /// Error is just Frobenius norm between rotation matrices. Vector evaluateError(const Rot& R1, const Rot& R2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { Vector error = R2.vec(H2) - R1.vec(H1); if (H1) *H1 = -*H1; return error; @@ -110,7 +110,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { /// Error is Frobenius norm between R1*R12 and R2. Vector evaluateError(const Rot& R1, const Rot& R2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { const Rot R2hat = R1.compose(R12_); Eigen::Matrix vec_H_R2hat; Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); @@ -139,7 +139,7 @@ class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2 /// projects down from SO(p) to the Stiefel manifold of px3 matrices. Vector evaluateError(const SOn& Q1, const SOn& Q2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + boost::optional H2 = boost::none) const override; }; } // namespace gtsam diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 0eb489f35..f848a56ca 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -96,7 +96,7 @@ public: virtual ~GeneralSFMFactor() {} ///< destructor /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this)));} @@ -105,7 +105,7 @@ public: * @param s optional string naming the factor * @param keyFormatter optional formatter for printing out Symbols */ - void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } @@ -113,14 +113,14 @@ public: /** * equals */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + bool equals(const NonlinearFactor &p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const { + boost::optional H1=boost::none, boost::optional H2=boost::none) const override { try { return camera.project2(point,H1,H2) - measured_; } @@ -133,7 +133,7 @@ public: } /// Linearize using fixed-size matrices - boost::shared_ptr linearize(const Values& values) const { + boost::shared_ptr linearize(const Values& values) const override { // Only linearize if the factor is active if (!this->active(values)) return boost::shared_ptr(); @@ -230,7 +230,7 @@ public: virtual ~GeneralSFMFactor2() {} ///< destructor /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this)));} @@ -239,7 +239,7 @@ public: * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } @@ -247,7 +247,7 @@ public: /** * equals */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + bool equals(const NonlinearFactor &p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } @@ -256,7 +256,7 @@ public: Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, boost::optional H1=boost::none, boost::optional H2=boost::none, - boost::optional H3=boost::none) const + boost::optional H3=boost::none) const override { try { Camera camera(pose3,calib); diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 1f56adc45..e83464b1e 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -41,13 +41,13 @@ public: } /// print - virtual void print(const std::string& s = "OrientedPlane3Factor", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "OrientedPlane3Factor", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /// evaluateError - virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, + Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { + boost::none) const override { OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2); Vector err(3); @@ -78,14 +78,14 @@ public: } /// print - virtual void print(const std::string& s = "OrientedPlane3DirectionPrior", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "OrientedPlane3DirectionPrior", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; - virtual Vector evaluateError(const OrientedPlane3& plane, - boost::optional H = boost::none) const; + Vector evaluateError(const OrientedPlane3& plane, + boost::optional H = boost::none) const override; }; } // gtsam diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index f4ce1520a..7e8bdaa46 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -50,7 +50,7 @@ public: virtual ~PoseRotationPrior() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -60,19 +60,19 @@ public: // testable /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); } /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s + "PoseRotationPrior", keyFormatter); measured_.print("Measured Rotation"); } /** h(x)-z */ - Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { + Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const override { const Rotation& newR = pose.rotation(); if (H) { *H = Matrix::Zero(rDim, xDim); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 516582d83..0c029c501 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -54,12 +54,12 @@ public: const Translation& measured() const { return measured_; } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** h(x)-z */ - Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { + Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const override { const Translation& newTrans = pose.translation(); const Rotation& R = pose.rotation(); const int tDim = traits::GetDimension(newTrans); @@ -74,13 +74,13 @@ public: } /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); } /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s + "PoseTranslationPrior", keyFormatter); traits::Print(measured_, "Measured Translation"); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 0bed15fdc..266037469 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -100,7 +100,7 @@ namespace gtsam { virtual ~GenericProjectionFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -109,7 +109,7 @@ namespace gtsam { * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "GenericProjectionFactor, z = "; traits::Print(measured_); if(this->body_P_sensor_) @@ -118,7 +118,7 @@ namespace gtsam { } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -129,7 +129,7 @@ namespace gtsam { /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { try { if(body_P_sensor_) { if(H1) { diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 30f761934..e41b5f908 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -89,23 +89,23 @@ public: virtual ~ReferenceFrameFactor(){} - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } /** Combined cost and derivative function using boost::optional */ - virtual Vector evaluateError(const Point& global, const Transform& trans, const Point& local, + Vector evaluateError(const Point& global, const Transform& trans, const Point& local, boost::optional Dforeign = boost::none, boost::optional Dtrans = boost::none, - boost::optional Dlocal = boost::none) const { + boost::optional Dlocal = boost::none) const override { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) *Dlocal = -1* Matrix::Identity(traits::dimension, traits::dimension); return traits::Local(local,newlocal); } - virtual void print(const std::string& s="", - const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s="", + const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": ReferenceFrameFactor(" << "Global: " << keyFormatter(this->key1()) << "," << " Transform: " << keyFormatter(this->key2()) << "," diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 948fffe13..9e4091111 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -36,13 +36,13 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << "RotateFactor:]\n"; std::cout << "p: " << p_.transpose() << std::endl; @@ -51,7 +51,7 @@ public: /// vector of errors returns 2D vector Vector evaluateError(const Rot3& R, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { // predict p_ as q = R*z_, derivative H will be filled if not none Point3 q = R.rotate(z_,H); // error is just difference, and note derivative of that wrpt q is I3 @@ -88,13 +88,13 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << "RotateDirectionsFactor:" << std::endl; i_p_.print("p"); @@ -102,7 +102,7 @@ public: } /// vector of errors returns 2D vector - Vector evaluateError(const Rot3& iRc, boost::optional H = boost::none) const { + Vector evaluateError(const Rot3& iRc, boost::optional H = boost::none) const override { Unit3 i_q = iRc * c_z_; Vector error = i_p_.error(i_q, H); if (H) { diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 1c80b8744..d9f2b9c3d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -150,7 +150,7 @@ protected: } /// get the dimension (number of rows!) of the factor - virtual size_t dim() const { + size_t dim() const override { return ZDim * this->measured_.size(); } @@ -173,7 +173,7 @@ protected: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + DefaultKeyFormatter) const override { std::cout << s << "SmartFactorBase, z = \n"; for (size_t k = 0; k < measured_.size(); ++k) { std::cout << "measurement, p = " << measured_[k] << "\t"; @@ -185,7 +185,7 @@ protected: } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); bool areMeasurementsEqual = true; diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 15d632cda..a7d85dd08 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -99,7 +99,7 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionFactor\n"; std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl; @@ -110,7 +110,7 @@ public: } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && params_.linearizationMode == e->params_.linearizationMode && Base::equals(p, tol); @@ -305,8 +305,8 @@ public: } /// linearize - virtual boost::shared_ptr linearize( - const Values& values) const { + boost::shared_ptr linearize( + const Values& values) const override { return linearizeDamped(values); } @@ -409,7 +409,7 @@ public: } /// Calculate total reprojection error - virtual double error(const Values& values) const { + double error(const Values& values) const override { if (this->active(values)) { return totalReprojectionError(Base::cameras(values)); } else { // else of active flag diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index b5be46258..cccdf20d6 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -103,13 +103,13 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionPoseFactor, z = \n "; Base::print("", keyFormatter); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol); } @@ -117,7 +117,7 @@ public: /** * error calculates the error of the factor. */ - virtual double error(const Values& values) const { + double error(const Values& values) const override { if (this->active(values)) { return this->totalReprojectionError(cameras(values)); } else { // else of active flag @@ -136,7 +136,7 @@ public: * to keys involved in this factor * @return vector of Values */ - typename Base::Cameras cameras(const Values& values) const { + typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; for (const Key& k : this->keys_) { const Pose3 world_P_sensor_k = diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 8828f5de7..6556723bd 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -91,7 +91,7 @@ public: virtual ~GenericStereoFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -100,7 +100,7 @@ public: * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); measured_.print(s + ".z"); if(this->body_P_sensor_) @@ -110,7 +110,7 @@ public: /** * equals */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const GenericStereoFactor* e = dynamic_cast (&f); return e && Base::equals(f) @@ -120,7 +120,7 @@ public: /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { try { if(body_P_sensor_) { if(H1) { diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 811d92fbc..9d02ad321 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -90,7 +90,7 @@ public: } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -101,7 +101,7 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + DefaultKeyFormatter) const override { std::cout << s << "TriangulationFactor,"; camera_.print("camera"); traits::Print(measured_, "z"); @@ -109,7 +109,7 @@ public: } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol) && traits::Equals(this->measured_, e->measured_, tol); @@ -117,7 +117,7 @@ public: /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Point3& point, boost::optional H2 = - boost::none) const { + boost::none) const override { try { return traits::Local(measured_, camera_.project2(point, boost::none, H2)); } catch (CheiralityException& e) { @@ -143,7 +143,7 @@ public: * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x) const { + boost::shared_ptr linearize(const Values& x) const override { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index fd771f102..951cbf8f4 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -41,9 +41,9 @@ public: boost::optional body_P_sensor = boost::none, size_t expectedNumberCameras = 10) : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} - virtual double error(const Values& values) const { return 0.0; } - virtual boost::shared_ptr linearize( - const Values& values) const { + double error(const Values& values) const override { return 0.0; } + boost::shared_ptr linearize( + const Values& values) const override { return boost::shared_ptr(new JacobianFactor()); } }; @@ -105,11 +105,11 @@ public: StereoFactor() {} StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { } - virtual double error(const Values& values) const { + double error(const Values& values) const override { return 0.0; } - virtual boost::shared_ptr linearize( - const Values& values) const { + boost::shared_ptr linearize( + const Values& values) const override { return boost::shared_ptr(new JacobianFactor()); } }; diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 0530af556..9163cdba6 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -105,7 +105,7 @@ namespace gtsam { /// @name Testable /** Print with optional formatter */ - void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check equality */ bool equals(const This& c, double tol = 1e-9) const; diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 3e7296593..80e700b29 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -34,11 +34,11 @@ namespace gtsam { AllDiff(const DiscreteKeys& dkeys); // print - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /// equals - bool equals(const DiscreteFactor& other, double tol) const { + bool equals(const DiscreteFactor& other, double tol) const override { if(!dynamic_cast(&other)) return false; else { @@ -50,13 +50,13 @@ namespace gtsam { } /// Calculate value = expensive ! - virtual double operator()(const Values& values) const; + double operator()(const Values& values) const override; /// Convert into a decisiontree, can be *very* expensive ! - virtual DecisionTreeFactor toDecisionTreeFactor() const; + DecisionTreeFactor toDecisionTreeFactor() const override; /// Multiply into a decisiontree - virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /* * Ensure Arc-consistency @@ -65,13 +65,13 @@ namespace gtsam { * @param j domain to be checked * @param domains all other domains */ - bool ensureArcConsistency(size_t j, std::vector& domains) const; + bool ensureArcConsistency(size_t j, std::vector& domains) const override; /// Partially apply known values - virtual Constraint::shared_ptr partiallyApply(const Values&) const; + Constraint::shared_ptr partiallyApply(const Values&) const override; /// Partially apply known values, domain version - virtual Constraint::shared_ptr partiallyApply(const std::vector&) const; + Constraint::shared_ptr partiallyApply(const std::vector&) const override; }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 1ae5a008a..bbb60e2f1 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -33,14 +33,14 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " << formatter(keys_[1]) << std::endl; } /// equals - bool equals(const DiscreteFactor& other, double tol) const { + bool equals(const DiscreteFactor& other, double tol) const override { if(!dynamic_cast(&other)) return false; else { @@ -50,12 +50,12 @@ namespace gtsam { } /// Calculate value - virtual double operator()(const Values& values) const { + double operator()(const Values& values) const override { return (double) (values.at(keys_[0]) != values.at(keys_[1])); } /// Convert into a decisiontree - virtual DecisionTreeFactor toDecisionTreeFactor() const { + DecisionTreeFactor toDecisionTreeFactor() const override { DiscreteKeys keys; keys.push_back(DiscreteKey(keys_[0],cardinality0_)); keys.push_back(DiscreteKey(keys_[1],cardinality1_)); @@ -68,7 +68,7 @@ namespace gtsam { } /// Multiply into a decisiontree - virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { // TODO: can we do this more efficiently? return toDecisionTreeFactor() * f; } @@ -79,20 +79,20 @@ namespace gtsam { * @param domains all other domains */ /// - bool ensureArcConsistency(size_t j, std::vector& domains) const { + bool ensureArcConsistency(size_t j, std::vector& domains) const override { // throw std::runtime_error( // "BinaryAllDiff::ensureArcConsistency not implemented"); return false; } /// Partially apply known values - virtual Constraint::shared_ptr partiallyApply(const Values&) const { + Constraint::shared_ptr partiallyApply(const Values&) const override { throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); } /// Partially apply known values, domain version - virtual Constraint::shared_ptr partiallyApply( - const std::vector&) const { + Constraint::shared_ptr partiallyApply( + const std::vector&) const override { throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); } }; diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 5dd597e20..5acc5a08f 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -66,11 +66,11 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /// equals - bool equals(const DiscreteFactor& other, double tol) const { + bool equals(const DiscreteFactor& other, double tol) const override { if(!dynamic_cast(&other)) return false; else { @@ -84,20 +84,20 @@ namespace gtsam { } /// Calculate value - virtual double operator()(const Values& values) const; + double operator()(const Values& values) const override; /// Convert into a decisiontree - virtual DecisionTreeFactor toDecisionTreeFactor() const; + DecisionTreeFactor toDecisionTreeFactor() const override; /// Multiply into a decisiontree - virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /* * Ensure Arc-consistency * @param j domain to be checked * @param domains all other domains */ - bool ensureArcConsistency(size_t j, std::vector& domains) const; + bool ensureArcConsistency(size_t j, std::vector& domains) const override; /** * Check for a value in domain that does not occur in any other connected domain. @@ -107,12 +107,11 @@ namespace gtsam { bool checkAllDiff(const KeyVector keys, std::vector& domains); /// Partially apply known values - virtual Constraint::shared_ptr partiallyApply( - const Values& values) const; + Constraint::shared_ptr partiallyApply(const Values& values) const override; /// Partially apply known values, domain version - virtual Constraint::shared_ptr partiallyApply( - const std::vector& domains) const; + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 43f06956d..c4d2addec 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -42,11 +42,11 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /// equals - bool equals(const DiscreteFactor& other, double tol) const { + bool equals(const DiscreteFactor& other, double tol) const override { if(!dynamic_cast(&other)) return false; else { @@ -56,28 +56,27 @@ namespace gtsam { } /// Calculate value - virtual double operator()(const Values& values) const; + double operator()(const Values& values) const override; /// Convert into a decisiontree - virtual DecisionTreeFactor toDecisionTreeFactor() const; + DecisionTreeFactor toDecisionTreeFactor() const override; /// Multiply into a decisiontree - virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /* * Ensure Arc-consistency * @param j domain to be checked * @param domains all other domains */ - bool ensureArcConsistency(size_t j, std::vector& domains) const; + bool ensureArcConsistency(size_t j, std::vector& domains) const override; /// Partially apply known values - virtual Constraint::shared_ptr partiallyApply( - const Values& values) const; + Constraint::shared_ptr partiallyApply(const Values& values) const override; /// Partially apply known values, domain version - virtual Constraint::shared_ptr partiallyApply( - const std::vector& domains) const; + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; }; } // namespace gtsam diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 1c5ade5b6..c8c351d7a 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -51,12 +51,12 @@ public: virtual ~FullIMUFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { + bool equals(const NonlinearFactor& e, double tol = 1e-9) const override { const This* const f = dynamic_cast(&e); return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && @@ -64,7 +64,7 @@ public: std::abs(dt_ - f->dt_) < tol; } - void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { + void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override { std::string a = "FullIMUFactor: " + s; Base::print(a, formatter); gtsam::print((Vector)accel_, "accel"); @@ -81,9 +81,9 @@ public: * Error evaluation with optional derivatives - calculates * z - h(x1,x2) */ - virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, + Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { Vector9 z; z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index bb0a354ee..fb864a78d 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -44,12 +44,12 @@ public: virtual ~IMUFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { + bool equals(const NonlinearFactor& e, double tol = 1e-9) const override { const This* const f = dynamic_cast(&e); return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && @@ -57,7 +57,7 @@ public: std::abs(dt_ - f->dt_) < tol; } - void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { + void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override { std::string a = "IMUFactor: " + s; Base::print(a, formatter); gtsam::print((Vector)accel_, "accel"); @@ -74,9 +74,9 @@ public: * Error evaluation with optional derivatives - calculates * z - h(x1,x2) */ - virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, + Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { const Vector6 meas = z(); if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 209456a62..470d7108b 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -40,7 +40,7 @@ public: : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } @@ -48,7 +48,7 @@ public: Vector evaluateError(const double& qk1, const double& qk, const double& v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { const size_t p = 1; if (H1) *H1 = -Matrix::Identity(p,p); if (H2) *H2 = Matrix::Identity(p,p); @@ -88,7 +88,7 @@ public: : Base(noiseModel::Constrained::All(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } @@ -96,7 +96,7 @@ public: Vector evaluateError(const double & vk1, const double & vk, const double & q, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { const size_t p = 1; if (H1) *H1 = -Matrix::Identity(p,p); if (H2) *H2 = Matrix::Identity(p,p); @@ -139,7 +139,7 @@ public: h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } @@ -147,7 +147,7 @@ public: Vector evaluateError(const double & pk, const double & qk, const double & qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { const size_t p = 1; double qmid = (1-alpha_)*qk + alpha_*qk1; @@ -195,7 +195,7 @@ public: h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } @@ -203,7 +203,7 @@ public: Vector evaluateError(const double & pk1, const double & qk, const double & qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { const size_t p = 1; double qmid = (1-alpha_)*qk + alpha_*qk1; diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index f38c256b1..42ef04f84 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -36,7 +36,7 @@ public: virtual ~Reconstruction() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); } @@ -44,7 +44,7 @@ public: Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { Matrix6 D_exphxi_xi; Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0); @@ -98,7 +98,7 @@ public: virtual ~DiscreteEulerPoincareHelicopter() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); } @@ -110,7 +110,7 @@ public: Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { Vector muk = Inertia_*xik; Vector muk_1 = Inertia_*xik_1; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index ed3797015..986d8e271 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -73,16 +73,16 @@ public: virtual ~VelocityConstraint() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); } /** * Calculates the error for trapezoidal model given */ - virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, + gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H2=boost::none) const override { if (H1) *H1 = gtsam::numericalDerivative21( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); if (H2) *H2 = gtsam::numericalDerivative22( @@ -90,7 +90,7 @@ public: return evaluateError_(x1, x2, dt_, integration_mode_); } - virtual void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { + void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override { std::string a = "VelocityConstraint: " + s; Base::print(a, formatter); switch(integration_mode_) { diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 721d0265b..f6cd8ccc4 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -31,7 +31,7 @@ public: virtual ~VelocityConstraint3() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } @@ -39,7 +39,7 @@ public: Vector evaluateError(const double& x1, const double& x2, const double& v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { const size_t p = 1; if (H1) *H1 = Matrix::Identity(p,p); if (H2) *H2 = -Matrix::Identity(p,p); diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h index 60adb872e..6f589d0c3 100644 --- a/gtsam_unstable/linear/InfeasibleInitialValues.h +++ b/gtsam_unstable/linear/InfeasibleInitialValues.h @@ -29,10 +29,10 @@ public: InfeasibleInitialValues() { } - virtual ~InfeasibleInitialValues() throw () { + virtual ~InfeasibleInitialValues() noexcept { } - virtual const char *what() const throw () { + const char *what() const noexcept override { if (description_.empty()) description_ = "An infeasible initial value was provided for the solver.\n"; diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h index 2184e9f52..25742d61f 100644 --- a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h +++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h @@ -25,10 +25,10 @@ class InfeasibleOrUnboundedProblem: public ThreadsafeException< public: InfeasibleOrUnboundedProblem() { } - virtual ~InfeasibleOrUnboundedProblem() throw () { + virtual ~InfeasibleOrUnboundedProblem() noexcept { } - virtual const char* what() const throw () { + const char* what() const noexcept override { if (description_.empty()) description_ = "The problem is either infeasible or unbounded.\n"; return description_.c_str(); diff --git a/gtsam_unstable/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h index b489510af..c22c389be 100644 --- a/gtsam_unstable/linear/LinearCost.h +++ b/gtsam_unstable/linear/LinearCost.h @@ -88,18 +88,18 @@ public: } /** equals */ - virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + bool equals(const GaussianFactor& lf, double tol = 1e-9) const override { return Base::equals(lf, tol); } /** print */ - virtual void print(const std::string& s = "", const KeyFormatter& formatter = - DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override { Base::print(s + " LinearCost: ", formatter); } /** Clone this LinearCost */ - virtual GaussianFactor::shared_ptr clone() const { + GaussianFactor::shared_ptr clone() const override { return boost::static_pointer_cast < GaussianFactor > (boost::make_shared < LinearCost > (*this)); } @@ -110,7 +110,7 @@ public: } /** Special error for single-valued inequality constraints. */ - virtual double error(const VectorValues& c) const { + double error(const VectorValues& c) const override { return error_vector(c)[0]; } }; diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index 2463ef31c..d960d8336 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -89,18 +89,18 @@ public: } /** equals */ - virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + bool equals(const GaussianFactor& lf, double tol = 1e-9) const override { return Base::equals(lf, tol); } /** print */ - virtual void print(const std::string& s = "", const KeyFormatter& formatter = - DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override { Base::print(s, formatter); } /** Clone this LinearEquality */ - virtual GaussianFactor::shared_ptr clone() const { + GaussianFactor::shared_ptr clone() const override { return boost::static_pointer_cast < GaussianFactor > (boost::make_shared < LinearEquality > (*this)); } @@ -124,7 +124,7 @@ public: * I think it should be zero, as this function is meant for objective cost. * But the name "error" can be misleading. * TODO: confirm with Frank!! */ - virtual double error(const VectorValues& c) const { + double error(const VectorValues& c) const override { return 0.0; } diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 1a31bd4e4..d353afc46 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -100,13 +100,13 @@ public: } /** equals */ - virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + bool equals(const GaussianFactor& lf, double tol = 1e-9) const override { return Base::equals(lf, tol); } /** print */ - virtual void print(const std::string& s = "", const KeyFormatter& formatter = - DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override { if (active()) Base::print(s + " Active", formatter); else @@ -114,7 +114,7 @@ public: } /** Clone this LinearInequality */ - virtual GaussianFactor::shared_ptr clone() const { + GaussianFactor::shared_ptr clone() const override { return boost::static_pointer_cast < GaussianFactor > (boost::make_shared < LinearInequality > (*this)); } @@ -145,7 +145,7 @@ public: } /** Special error for single-valued inequality constraints. */ - virtual double error(const VectorValues& c) const { + double error(const VectorValues& c) const override { return error_vector(c)[0]; } diff --git a/gtsam_unstable/linear/QPSParserException.h b/gtsam_unstable/linear/QPSParserException.h index ed4c79bdd..ef14ed430 100644 --- a/gtsam_unstable/linear/QPSParserException.h +++ b/gtsam_unstable/linear/QPSParserException.h @@ -25,10 +25,10 @@ public: QPSParserException() { } - virtual ~QPSParserException() throw () { + virtual ~QPSParserException() noexcept { } - virtual const char *what() const throw () { + const char *what() const noexcept override { if (description_.empty()) description_ = "There is a problem parsing the QPS file.\n"; return description_.c_str(); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index d71cdd409..2291642aa 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -41,10 +41,10 @@ public: virtual ~BatchFixedLagSmoother() { }; /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two IncrementalFixedLagSmoother Objects are equal */ - virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override; + bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override; /** Add new factors, updating the solution and relinearizing as needed. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index d04f579eb..c99c67492 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -67,10 +67,10 @@ public: virtual ~ConcurrentBatchFilter() {}; /** Implement a GTSAM standard 'print' function */ - virtual void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two Concurrent Filters are equal */ - virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const; + bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override; /** Access the current set of factors */ const NonlinearFactorGraph& getFactors() const { @@ -130,7 +130,7 @@ public: * Perform any required operations before the synchronization process starts. * Called by 'synchronize' */ - virtual void presync(); + virtual void presync() override; /** * Populate the provided containers with factors that constitute the filter branch summarization @@ -139,7 +139,7 @@ public: * @param summarizedFactors The summarized factors for the filter branch * @param rootValues The linearization points of the root clique variables */ - virtual void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues); + void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override; /** * Populate the provided containers with factors being sent to the smoother from the filter. These @@ -149,20 +149,20 @@ public: * @param smootherFactors The new factors to be added to the smoother * @param smootherValues The linearization points of any new variables */ - virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues); + void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override; /** * Apply the updated version of the smoother branch summarized factors. * * @param summarizedFactors An updated version of the smoother branch summarized factors */ - virtual void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues); + void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override; /** * Perform any required operations after the synchronization process finishes. * Called by 'synchronize' */ - virtual void postsync(); + virtual void postsync() override; protected: diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 67a1ef4f3..364d02caf 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -60,10 +60,10 @@ public: virtual ~ConcurrentBatchSmoother() {}; /** Implement a GTSAM standard 'print' function */ - virtual void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two Concurrent Smoothers are equal */ - virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const; + bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override; /** Access the current set of factors */ const NonlinearFactorGraph& getFactors() const { @@ -124,7 +124,7 @@ public: * Perform any required operations before the synchronization process starts. * Called by 'synchronize' */ - virtual void presync(); + virtual void presync() override; /** * Populate the provided containers with factors that constitute the smoother branch summarization @@ -132,7 +132,7 @@ public: * * @param summarizedFactors The summarized factors for the filter branch */ - virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues); + void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override; /** * Apply the new smoother factors sent by the filter, and the updated version of the filter @@ -143,14 +143,14 @@ public: * @param summarizedFactors An updated version of the filter branch summarized factors * @param rootValues The linearization point of the root variables */ - virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, - const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues); + void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, + const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override; /** * Perform any required operations after the synchronization process finishes. * Called by 'synchronize' */ - virtual void postsync(); + virtual void postsync() override; protected: diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index e01919048..d0525a4da 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -67,10 +67,10 @@ public: virtual ~ConcurrentIncrementalFilter() {}; /** Implement a GTSAM standard 'print' function */ - virtual void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two Concurrent Filters are equal */ - virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const; + bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override; /** Access the current set of factors */ const NonlinearFactorGraph& getFactors() const { @@ -130,7 +130,7 @@ public: * Perform any required operations before the synchronization process starts. * Called by 'synchronize' */ - virtual void presync(); + void presync() override; /** * Populate the provided containers with factors that constitute the filter branch summarization @@ -139,7 +139,7 @@ public: * @param summarizedFactors The summarized factors for the filter branch * @param rootValues The linearization points of the root clique variables */ - virtual void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues); + void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override; /** * Populate the provided containers with factors being sent to the smoother from the filter. These @@ -149,20 +149,20 @@ public: * @param smootherFactors The new factors to be added to the smoother * @param smootherValues The linearization points of any new variables */ - virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues); + void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override; /** * Apply the updated version of the smoother branch summarized factors. * * @param summarizedFactors An updated version of the smoother branch summarized factors */ - virtual void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues); + void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override; /** * Perform any required operations after the synchronization process finishes. * Called by 'synchronize' */ - virtual void postsync(); + void postsync() override; protected: diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index ec95e1ec8..8848b6a43 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -57,10 +57,10 @@ public: virtual ~ConcurrentIncrementalSmoother() {}; /** Implement a GTSAM standard 'print' function */ - virtual void print(const std::string& s = "Concurrent Incremental Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "Concurrent Incremental Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two Concurrent Smoothers are equal */ - virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const; + bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override; /** Access the current set of factors */ const NonlinearFactorGraph& getFactors() const { @@ -115,7 +115,7 @@ public: * Perform any required operations before the synchronization process starts. * Called by 'synchronize' */ - virtual void presync(); + void presync() override; /** * Populate the provided containers with factors that constitute the smoother branch summarization @@ -123,7 +123,7 @@ public: * * @param summarizedFactors The summarized factors for the filter branch */ - virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues); + void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override; /** * Apply the new smoother factors sent by the filter, and the updated version of the filter @@ -134,14 +134,14 @@ public: * @param summarizedFactors An updated version of the filter branch summarized factors * @param rootValues The linearization point of the root variables */ - virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, - const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues); + void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, + const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override; /** * Perform any required operations after the synchronization process finishes. * Called by 'synchronize' */ - virtual void postsync(); + void postsync() override; protected: diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 128889b82..caf67de21 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -112,17 +112,17 @@ public: virtual ~LinearizedJacobianFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // Testable /** print function */ - virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** equals function with optional tolerance */ - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; // access functions const constBVector b() const { return Ab_(size()).col(0); } @@ -130,17 +130,17 @@ public: const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { return Ab_.rows(); }; + size_t dim() const override { return Ab_.rows(); }; /** Calculate the error of the factor */ - double error(const Values& c) const; + double error(const Values& c) const override; /** * linearize to a GaussianFactor * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize(const Values& c) const; + boost::shared_ptr linearize(const Values& c) const override; /** (A*x-b) */ Vector error_vector(const Values& c) const; @@ -202,17 +202,17 @@ public: virtual ~LinearizedHessianFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // Testable /** print function */ - virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** equals function with optional tolerance */ - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; /** Return the constant term \f$ f \f$ as described above * @return The constant term \f$ f \f$ @@ -261,17 +261,17 @@ public: } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { return info_.rows() - 1; } + size_t dim() const override { return info_.rows() - 1; } /** Calculate the error of the factor */ - double error(const Values& c) const; + double error(const Values& c) const override; /** * linearize to a GaussianFactor * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize(const Values& c) const; + boost::shared_ptr linearize(const Values& c) const override; private: /** Serialization function */ diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 104b3209d..4104ba653 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -84,8 +84,8 @@ public: /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override { std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << "," << keyFormatter(key2_) << ")\n"; measured_.print(" measured: "); @@ -97,7 +97,7 @@ public: } /** equals */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const This *t = dynamic_cast(&f); if (t && Base::equals(f)) @@ -408,7 +408,7 @@ public: return 2; } - virtual size_t dim() const { + size_t dim() const override { return model_inlier_->R().rows() + model_inlier_->R().cols(); } diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index cf56afab2..c8bbdd78a 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -55,7 +55,7 @@ namespace gtsam { /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BiasedGPSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n" @@ -64,7 +64,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } @@ -74,7 +74,7 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const Pose3& pose, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { + boost::none) const override { if (H1 || H2){ H1->resize(3,6); // jacobian wrt pose diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index 574efabea..08b1c800a 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -34,10 +34,10 @@ public: // testable /** print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override; // access @@ -48,13 +48,13 @@ public: /** * Calculate the error of the factor - zero for dummy factors */ - virtual double error(const Values& c) const { return 0.0; } + double error(const Values& c) const override { return 0.0; } /** get the dimension of the factor (number of rows on linearization) */ - virtual size_t dim() const { return rowDim_; } + size_t dim() const override { return rowDim_; } /** linearize to a GaussianFactor */ - virtual boost::shared_ptr linearize(const Values& c) const; + boost::shared_ptr linearize(const Values& c) const override; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -62,7 +62,7 @@ public: * * By default, throws exception if subclass does not implement the function. */ - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new DummyFactor(*this))); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index f499a0f4b..745605325 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -133,7 +133,7 @@ public: /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," @@ -153,7 +153,7 @@ public: } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol @@ -302,7 +302,7 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const { + boost::optional H5 = boost::none) const override { // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 86efd10c1..9f5d800db 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -151,7 +151,7 @@ public: } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 762199753..f6de48625 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -71,7 +71,7 @@ public: /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "GaussMarkov1stOrderFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; @@ -79,7 +79,7 @@ public: } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol); } @@ -89,7 +89,7 @@ public: /** vector of errors */ Vector evaluateError(const VALUE& p1, const VALUE& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { Vector v1( traits::Logmap(p1) ); Vector v2( traits::Logmap(p2) ); diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 4763c4263..e5a3e9118 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -114,7 +114,7 @@ public: /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," @@ -133,7 +133,7 @@ public: } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && (measurement_acc_ - e->measurement_acc_).norm() < tol @@ -229,7 +229,7 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const { + boost::optional H5 = boost::none) const override { // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 0d10b0123..50b0c5980 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -70,13 +70,13 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactor3", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -85,7 +85,7 @@ public: Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, boost::optional H1=boost::none, boost::optional H2=boost::none, - boost::optional H3=boost::none) const { + boost::optional H3=boost::none) const override { try { InvDepthCamera3 camera(pose, K_); return camera.project(point, invDepth, H1, H2, H3) - measured_; diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index ad66eee5b..785556750 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -66,13 +66,13 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant1", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -102,7 +102,7 @@ public: /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector6& landmark, boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H2=boost::none) const override { if (H1) { (*H1) = numericalDerivative11( diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index c6b5d5af8..97ceb8a8b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -69,13 +69,13 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant2", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -105,7 +105,7 @@ public: /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector3& landmark, boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H2=boost::none) const override { if (H1) { (*H1) = numericalDerivative11( diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 83df9e13b..21c7aa6de 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -68,13 +68,13 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant3a", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -105,7 +105,7 @@ public: /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector3& landmark, boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H2=boost::none) const override { if(H1) { (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); @@ -188,13 +188,13 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant3", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -226,7 +226,7 @@ public: Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none, - boost::optional H3=boost::none) const { + boost::optional H3=boost::none) const override { if(H1) (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 5bef97bcf..b6bf2a9c7 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -101,7 +101,7 @@ namespace gtsam { virtual ~MultiProjectionFactor() {} /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } @@ -110,7 +110,7 @@ namespace gtsam { * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "MultiProjectionFactor, z = "; std::cout << measured_ << "measurements, z = "; if(this->body_P_sensor_) @@ -119,7 +119,7 @@ namespace gtsam { } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -129,7 +129,7 @@ namespace gtsam { } /// Evaluate error h(x)-z and optionally derivatives - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { Vector a; return a; diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index c6d892e93..d284366e8 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -85,20 +85,20 @@ namespace gtsam { } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); gtsam::print(prior_, "prior"); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && @@ -108,7 +108,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& p, boost::optional H = boost::none) const { + Vector evaluateError(const T& p, boost::optional H = boost::none) const override { if (H) (*H) = H_; // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index bb5835f80..e20792e25 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -59,14 +59,14 @@ namespace gtsam { virtual ~PoseBetweenFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; @@ -77,7 +77,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) @@ -90,7 +90,7 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const POSE& p1, const POSE& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { if(body_P_sensor_) { POSE hx; if(H1 || H2) { diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index ce9861160..c9965f9bf 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -56,14 +56,14 @@ namespace gtsam { } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; prior_.print(" prior mean: "); if(this->body_P_sensor_) @@ -72,7 +72,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This* e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) @@ -83,7 +83,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const POSE& p, boost::optional H = boost::none) const { + Vector evaluateError(const POSE& p, boost::optional H = boost::none) const override { if(body_P_sensor_) { // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.localCoordinates(p.compose(*body_P_sensor_, H)); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 19b8d56e6..caa388e2f 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -96,7 +96,7 @@ namespace gtsam { virtual ~ProjectionFactorPPP() {} /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } @@ -105,14 +105,14 @@ namespace gtsam { * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "ProjectionFactorPPP, z = "; traits::Print(measured_); Base::print("", keyFormatter); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -124,7 +124,7 @@ namespace gtsam { Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { + boost::optional H3 = boost::none) const override { try { if(H1 || H2 || H3) { Matrix H0, H02; diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 369075abf..af3294bce 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -91,7 +91,7 @@ namespace gtsam { virtual ~ProjectionFactorPPPC() {} /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { + NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } @@ -100,14 +100,14 @@ namespace gtsam { * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "ProjectionFactorPPPC, z = "; traits::Print(measured_); Base::print("", keyFormatter); } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) @@ -119,7 +119,7 @@ namespace gtsam { boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const { + boost::optional H4 = boost::none) const override { try { if(H1 || H2 || H3 || H4) { Matrix H0, H02; diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index 3507a4492..b713d45dc 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -43,22 +43,22 @@ public: virtual ~RelativeElevationFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override; /** return the measured */ inline double measured() const { return measured_; } /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override; /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; private: diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 5511a0209..6bab3f334 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -73,14 +73,14 @@ class SmartRangeFactor: public NoiseModelFactor { // Testable /** print */ - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartRangeFactor with " << size() << " measurements\n"; NoiseModelFactor::print(s); } /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { return false; } @@ -143,8 +143,8 @@ class SmartRangeFactor: public NoiseModelFactor { /** * Error function *without* the NoiseModel, \f$ z-h(x) \f$. */ - virtual Vector unwhitenedError(const Values& x, - boost::optional&> H = boost::none) const { + Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const override { size_t n = size(); if (n < 3) { if (H) { @@ -175,7 +175,7 @@ class SmartRangeFactor: public NoiseModelFactor { } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index db80956fa..ac063eff9 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -102,7 +102,7 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + DefaultKeyFormatter) const override { std::cout << s << "SmartStereoProjectionFactor\n"; std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl; std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl; @@ -111,7 +111,7 @@ public: } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const SmartStereoProjectionFactor *e = dynamic_cast(&p); return e && params_.linearizationMode == e->params_.linearizationMode @@ -327,8 +327,8 @@ public: } /// linearize - virtual boost::shared_ptr linearize( - const Values& values) const { + boost::shared_ptr linearize( + const Values& values) const override { return linearizeDamped(values); } @@ -438,7 +438,7 @@ public: } /// Calculate total reprojection error - virtual double error(const Values& values) const { + double error(const Values& values) const override { if (this->active(values)) { return totalReprojectionError(Base::cameras(values)); } else { // else of active flag @@ -449,9 +449,9 @@ public: /** * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) */ - virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, + void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, - boost::optional E = boost::none) const + boost::optional E = boost::none) const override { // when using stereo cameras, some of the measurements might be missing: for(size_t i=0; i < cameras.size(); i++){ diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 7ea5a4c2f..124e45005 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -120,7 +120,7 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + DefaultKeyFormatter) const override { std::cout << s << "SmartStereoProjectionPoseFactor, z = \n "; for(const boost::shared_ptr& K: K_all_) K->print("calibration = "); @@ -128,7 +128,7 @@ public: } /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const SmartStereoProjectionPoseFactor *e = dynamic_cast(&p); @@ -138,7 +138,7 @@ public: /** * error calculates the error of the factor. */ - virtual double error(const Values& values) const { + double error(const Values& values) const override { if (this->active(values)) { return this->totalReprojectionError(cameras(values)); } else { // else of active flag @@ -157,7 +157,7 @@ public: * to keys involved in this factor * @return vector of Values */ - Base::Cameras cameras(const Values& values) const { + Base::Cameras cameras(const Values& values) const override { Base::Cameras cameras; size_t i=0; for(const Key& k: this->keys_) { diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index 0a456e59c..6f98a2dbd 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -47,7 +47,7 @@ public: /// Evaluate measurement error h(x)-z Vector evaluateError(const Pose2& pose, const Point2& point, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { + boost::none) const override { return pose.transformTo(point, H1, H2) - measured_; } }; @@ -79,7 +79,7 @@ public: boost::optional H1 = boost::none, // boost::optional H2 = boost::none, // boost::optional H3 = boost::none, // - boost::optional H4 = boost::none) const { + boost::optional H4 = boost::none) const override { if (H1 || H2 || H3 || H4) { // TODO use fixed-size matrices Matrix D_pose_g_base1, D_pose_g_pose; @@ -134,7 +134,7 @@ public: boost::optional H1 = boost::none, // boost::optional H2 = boost::none, // boost::optional H3 = boost::none, // - boost::optional H4 = boost::none) const { + boost::optional H4 = boost::none) const override { if (H1 || H2 || H3 || H4) { // TODO use fixed-size matrices Matrix D_pose1_g_base1, D_pose1_g_pose1; diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index c29e3e794..35080bd42 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -81,13 +81,13 @@ namespace gtsam { /** Clone */ - virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared(*this); } + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::make_shared(*this); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "TransformBtwRobotsUnaryFactor(" << keyFormatter(key_) << ")\n"; std::cout << "MR between factor keys: " @@ -99,7 +99,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { + bool equals(const NonlinearFactor& f, double tol=1e-9) const override { const This *t = dynamic_cast (&f); if(t && Base::equals(f)) @@ -128,7 +128,7 @@ namespace gtsam { } /* ************************************************************************* */ - virtual double error(const gtsam::Values& x) const { + double error(const gtsam::Values& x) const override { return whitenedError(x).squaredNorm(); } @@ -139,7 +139,7 @@ namespace gtsam { * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ /* This version of linearize recalculates the noise model each time */ - virtual boost::shared_ptr linearize(const gtsam::Values& x) const { + boost::shared_ptr linearize(const gtsam::Values& x) const override { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); @@ -208,7 +208,7 @@ namespace gtsam { return 1; } - virtual size_t dim() const { + size_t dim() const override { return model_->R().rows() + model_->R().cols(); } diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 48aaa8ed7..2db2844ae 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -97,13 +97,13 @@ namespace gtsam { /** Clone */ - virtual NonlinearFactor::shared_ptr clone() const { return boost::make_shared(*this); } + NonlinearFactor::shared_ptr clone() const override { return boost::make_shared(*this); } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "TransformBtwRobotsUnaryFactorEM(" << keyFormatter(key_) << ")\n"; std::cout << "MR between factor keys: " @@ -119,7 +119,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { + bool equals(const NonlinearFactor& f, double tol=1e-9) const override { const This *t = dynamic_cast (&f); if(t && Base::equals(f)) @@ -151,7 +151,7 @@ namespace gtsam { } /* ************************************************************************* */ - virtual double error(const Values& x) const { + double error(const Values& x) const override { return whitenedError(x).squaredNorm(); } @@ -162,7 +162,7 @@ namespace gtsam { * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ /* This version of linearize recalculates the noise model each time */ - virtual boost::shared_ptr linearize(const Values& x) const { + boost::shared_ptr linearize(const Values& x) const override { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); @@ -406,7 +406,7 @@ namespace gtsam { return 1; } - virtual size_t dim() const { + size_t dim() const override { return model_inlier_->R().rows() + model_inlier_->R().cols(); } diff --git a/tests/simulated2D.h b/tests/simulated2D.h index c4930a55b..ed412bba8 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -139,14 +139,14 @@ namespace simulated2D { } /// Return error and optional derivative - Vector evaluateError(const Pose& x, boost::optional H = boost::none) const { + Vector evaluateError(const Pose& x, boost::optional H = boost::none) const override { return (prior(x, H) - measured_); } virtual ~GenericPrior() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -185,14 +185,14 @@ namespace simulated2D { /// Evaluate error and optionally return derivatives Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { return (odo(x1, x2, H1, H2) - measured_); } virtual ~GenericOdometry() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -232,14 +232,14 @@ namespace simulated2D { /// Evaluate error and optionally return derivatives Vector evaluateError(const Pose& x1, const Landmark& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { return (mea(x1, x2, H1, H2) - measured_); } virtual ~GenericMeasurement() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index deb9292f7..cb8c09fc8 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -60,7 +60,7 @@ namespace simulated2D { virtual ~ScalarCoordConstraint1() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -87,8 +87,8 @@ namespace simulated2D { * @param x is the estimate of the constrained variable being evaluated * @param H is an optional Jacobian, linearized at x */ - virtual double value(const Point& x, boost::optional H = - boost::none) const { + double value(const Point& x, boost::optional H = + boost::none) const override { if (H) { Matrix D = Matrix::Zero(1, traits::GetDimension(x)); D(0, IDX) = 1.0; @@ -128,7 +128,7 @@ namespace simulated2D { virtual ~MaxDistanceConstraint() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -150,9 +150,9 @@ namespace simulated2D { * @param H2 is an optional Jacobian in x2 * @return the distance between the variables */ - virtual double value(const Point& x1, const Point& x2, + double value(const Point& x1, const Point& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); return range_trait(x1, x2); @@ -177,7 +177,7 @@ namespace simulated2D { virtual ~MinDistanceConstraint() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -200,9 +200,9 @@ namespace simulated2D { * @param H2 is an optional Jacobian in x2 * @return the distance between the variables */ - virtual double value(const Pose& x1, const Point& x2, + double value(const Pose& x1, const Point& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); return range_trait(x1, x2); diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index 948a87ce5..27932415b 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -119,12 +119,12 @@ namespace simulated2DOriented { /// Evaluate error and optionally derivative Vector evaluateError(const VALUE& x1, const VALUE& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + boost::optional H2 = boost::none) const override { return measured_.localCoordinates(odo(x1, x2, H1, H2)); } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 3c92e733e..3b4afb106 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -90,7 +90,7 @@ struct PointPrior3D: public NoiseModelFactor1 { * @return Vector error between prior value and x (Dimension: 3) */ Vector evaluateError(const Point3& x, boost::optional H = - boost::none) const { + boost::none) const override { return prior(x, H) - measured_; } }; @@ -121,7 +121,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { * @return vector error between measurement and prediction (Dimension: 3) */ Vector evaluateError(const Point3& x1, const Point3& x2, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { return mea(x1, x2, H1, H2) - measured_; } }; diff --git a/tests/smallExample.h b/tests/smallExample.h index 2b29a6d10..0c933d106 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -337,7 +337,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { gtsam::NoiseModelFactor1(model, key), z_(z) { } - Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const override { if (A) *A = H(x); return (h(x) - z_); } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index d759e83e1..0ea507a36 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -153,14 +153,14 @@ public: } /* print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": NonlinearMotionModel\n"; std::cout << " TestKey1: " << keyFormatter(key1()) << std::endl; std::cout << " TestKey2: " << keyFormatter(key2()) << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const This *e = dynamic_cast (&f); return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2()); } @@ -169,13 +169,13 @@ public: * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const Values& c) const { + double error(const Values& c) const override { Vector w = whitenedError(c); return 0.5 * w.dot(w); } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { + size_t dim() const override { return 2; } @@ -189,7 +189,7 @@ public: * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c) const { + boost::shared_ptr linearize(const Values& c) const override { const X1& x1 = c.at(key1()); const X2& x2 = c.at(key2()); Matrix A1, A2; @@ -212,7 +212,7 @@ public: /** vector of errors */ Vector evaluateError(const Point2& p1, const Point2& p2, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { + boost::none) const override { // error = p2 - f(p1) // H1 = d error / d p1 = -d f/ d p1 = -F @@ -284,13 +284,13 @@ public: } /* print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": NonlinearMeasurementModel\n"; std::cout << " TestKey: " << keyFormatter(key()) << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const This *e = dynamic_cast (&f); return (e != nullptr) && Base::equals(f); } @@ -299,13 +299,13 @@ public: * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const Values& c) const { + double error(const Values& c) const override { Vector w = whitenedError(c); return 0.5 * w.dot(w); } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { + size_t dim() const override { return 1; } @@ -319,7 +319,7 @@ public: * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z * Hence b = z - h(x1) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c) const { + boost::shared_ptr linearize(const Values& c) const override { const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); @@ -336,7 +336,7 @@ public: } /** vector of errors */ - Vector evaluateError(const Point2& p, boost::optional H1 = boost::none) const { + Vector evaluateError(const Point2& p, boost::optional H1 = boost::none) const override { // error = z - h(p) // H = d error / d p = -d h/ d p = -H Vector z_hat = h(p); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index e1156ceba..662b071df 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -239,12 +239,12 @@ public: typedef NoiseModelFactor4 Base; TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} - virtual Vector + Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const { + boost::optional H4 = boost::none) const override { if(H1) { *H1 = (Matrix(1, 1) << 1.0).finished(); *H2 = (Matrix(1, 1) << 2.0).finished(); @@ -254,7 +254,7 @@ public: return (Vector(1) << x1 + x2 + x3 + x4).finished(); } - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); } }; @@ -287,13 +287,13 @@ public: typedef NoiseModelFactor5 Base; TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {} - virtual Vector + Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const { + boost::optional H5 = boost::none) const override { if(H1) { *H1 = (Matrix(1, 1) << 1.0).finished(); *H2 = (Matrix(1, 1) << 2.0).finished(); @@ -336,14 +336,14 @@ public: typedef NoiseModelFactor6 Base; TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {} - virtual Vector + Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const { + boost::optional H6 = boost::none) const override { if(H1) { *H1 = (Matrix(1, 1) << 1.0).finished(); *H2 = (Matrix(1, 1) << 2.0).finished(); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index a549dc726..2616ab103 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -510,8 +510,8 @@ class IterativeLM : public LevenbergMarquardtOptimizer { initial_(initialValues) {} /// Solve that uses conjugate gradient - virtual VectorValues solve(const GaussianFactorGraph& gfg, - const NonlinearOptimizerParams& params) const { + VectorValues solve(const GaussianFactorGraph& gfg, + const NonlinearOptimizerParams& params) const override { VectorValues zeros = initial_.zeroVectors(); return conjugateGradientDescent(gfg, zeros, cgParams_); } diff --git a/wrap/Method.h b/wrap/Method.h index 88a73cd80..4d3c8d909 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -37,7 +37,7 @@ public: boost::optional instName = boost::none, bool verbose = false); - virtual bool isStatic() const { + bool isStatic() const override { return false; } @@ -64,10 +64,10 @@ public: private: // Emit method header - void proxy_header(FileWriter& proxyFile) const; + void proxy_header(FileWriter& proxyFile) const override; - virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args) const; + std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args) const override; }; } // \namespace wrap diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index ba5086507..8d78bb48f 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -30,7 +30,7 @@ struct ReturnType : public Qualified { ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) : Qualified(name, c), isPtr(ptr) {} - virtual void clear() { + void clear() override { Qualified::clear(); isPtr = false; } diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 8392a1cc5..dbb918596 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -41,10 +41,10 @@ struct StaticMethod: public MethodBase { protected: - virtual void proxy_header(FileWriter& proxyFile) const; + void proxy_header(FileWriter& proxyFile) const override; - virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args) const; + std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args) const override; }; } // \namespace wrap diff --git a/wrap/utilities.h b/wrap/utilities.h index 17c5cba12..8c6f9a0b2 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -40,7 +40,7 @@ class CantOpenFile : public std::exception { public: CantOpenFile(const std::string& filename) : what_("Can't open file " + filename) {} ~CantOpenFile() throw() {} - virtual const char* what() const throw() { return what_.c_str(); } + const char* what() const noexcept override { return what_.c_str(); } }; class OutputError : public std::exception { @@ -49,7 +49,7 @@ private: public: OutputError(const std::string& what) : what_(what) {} ~OutputError() throw() {} - virtual const char* what() const throw() { return what_.c_str(); } + const char* what() const noexcept override { return what_.c_str(); } }; class ParseFailed : public std::exception { @@ -58,7 +58,7 @@ class ParseFailed : public std::exception { public: ParseFailed(int length) : what_((boost::format("Parse failed at character [%d]")%(length-1)).str()) {} ~ParseFailed() throw() {} - virtual const char* what() const throw() { return what_.c_str(); } + const char* what() const noexcept override { return what_.c_str(); } }; class DependencyMissing : public std::exception { @@ -68,7 +68,7 @@ public: DependencyMissing(const std::string& dep, const std::string& loc) : what_("Missing dependency '" + dep + "' in " + loc) {} ~DependencyMissing() throw() {} - virtual const char* what() const throw() { return what_.c_str(); } + const char* what() const noexcept override { return what_.c_str(); } }; class DuplicateDefinition : public std::exception { @@ -78,7 +78,7 @@ public: DuplicateDefinition(const std::string& name) : what_("Duplicate definition of " + name) {} ~DuplicateDefinition() throw() {} - virtual const char* what() const throw() { return what_.c_str(); } + const char* what() const noexcept override { return what_.c_str(); } }; class AttributeError : public std::exception { @@ -88,7 +88,7 @@ public: AttributeError(const std::string& name, const std::string& problem) : what_("Class " + name + ": " + problem) {} ~AttributeError() throw() {} - virtual const char* what() const throw() { return what_.c_str(); } + const char* what() const noexcept override { return what_.c_str(); } }; // "Unique" key to signal calling the matlab object constructor with a raw pointer From 64fb7b9503fec55935bccd3cb4f1f8f12561baf8 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sun, 26 Jul 2020 23:35:11 +0200 Subject: [PATCH 21/32] Avoid -Woverride in clang <12.0.0 --- cmake/GtsamBuildTypes.cmake | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index bffe97904..7ba38e80b 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -104,7 +104,16 @@ if(MSVC) set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") else() # Common to all configurations, next for each configuration: - + + message (STATUS "CMAKE_CXX_COMPILER_VERSION: ${CMAKE_CXX_COMPILER_VERSION}") + + if ( + ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR + (CMAKE_CXX_COMPILER_ID MATCHES "GNU") + ) + set(flag_override_ -Wsuggest-override -Werror=suggest-override) + endif() + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall # Enable common warnings -fPIC # ensure proper code generation for shared libraries @@ -112,7 +121,7 @@ else() $<$:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address -Wreturn-type -Werror=return-type # Error on missing return() -Wformat -Werror=format-security # Error on wrong printf() arguments - $<$:-Wsuggest-override -Werror=suggest-override> # Enforce the use of the override keyword + $<$:${flag_override_}> # Enforce the use of the override keyword # CACHE STRING "(User editable) Private compiler flags for all configurations.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.") From 99256c6d286bf3b5f269d013eed9b37c58d41ffd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sun, 26 Jul 2020 23:35:49 +0200 Subject: [PATCH 22/32] relax override -Werror for now --- cmake/GtsamBuildTypes.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 7ba38e80b..53b21647d 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -111,7 +111,7 @@ else() ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR (CMAKE_CXX_COMPILER_ID MATCHES "GNU") ) - set(flag_override_ -Wsuggest-override -Werror=suggest-override) + set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday endif() set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON From 406060b457d59c2dda74f0045a5227af18496698 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 27 Jul 2020 00:04:57 +0200 Subject: [PATCH 23/32] Fix missing virtual dtors --- gtsam/discrete/DiscreteBayesTree.h | 1 + gtsam/linear/GaussianBayesTree.h | 1 + gtsam/nonlinear/ISAM2Clique.h | 1 + gtsam/symbolic/SymbolicBayesTree.h | 1 + 4 files changed, 4 insertions(+) diff --git a/gtsam/discrete/DiscreteBayesTree.h b/gtsam/discrete/DiscreteBayesTree.h index 3d6e016fd..29da5817e 100644 --- a/gtsam/discrete/DiscreteBayesTree.h +++ b/gtsam/discrete/DiscreteBayesTree.h @@ -45,6 +45,7 @@ class GTSAM_EXPORT DiscreteBayesTreeClique typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; DiscreteBayesTreeClique() {} + virtual ~DiscreteBayesTreeClique() {} DiscreteBayesTreeClique( const boost::shared_ptr& conditional) : Base(conditional) {} diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index b6f5acd52..e2d8ae87a 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -41,6 +41,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; GaussianBayesTreeClique() {} + virtual ~GaussianBayesTreeClique() {} GaussianBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} }; diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index 467741d66..9b40c02c6 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -51,6 +51,7 @@ class GTSAM_EXPORT ISAM2Clique /// Default constructor ISAM2Clique() : Base() {} + virtual ~ISAM2Clique() = default; /// Copy constructor, does *not* copy solution pointers as these are invalid /// in different trees. diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index e28f28764..3fdf1011b 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -39,6 +39,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; SymbolicBayesTreeClique() {} + virtual ~SymbolicBayesTreeClique() {} SymbolicBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} }; From 816529170478bb732c5269835a269d8a9fd6b056 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 27 Jul 2020 00:11:51 +0200 Subject: [PATCH 24/32] Fix warnings on incorrect for range reference bindings --- gtsam/base/timing.cpp | 2 +- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/Potentials.cpp | 2 +- gtsam/linear/Preconditioner.cpp | 2 +- gtsam/linear/VectorValues.cpp | 2 +- gtsam/slam/dataset.cpp | 4 ++-- wrap/Module.cpp | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index b33f06045..4b41ea937 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -89,7 +89,7 @@ void TimingOutline::print(const std::string& outline) const { childOrder[child.second->myOrder_] = child.second; } // Print children - for(const ChildOrder::value_type order_child: childOrder) { + for(const ChildOrder::value_type& order_child: childOrder) { std::string childOutline(outline); childOutline += "| "; order_child.second->print(childOutline); diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index f83349436..b7b9d7034 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -69,7 +69,7 @@ namespace gtsam { for(Key j: f.keys()) cs[j] = f.cardinality(j); // Convert map into keys DiscreteKeys keys; - for(const DiscreteKey& key: cs) + for(const std::pair& key: cs) keys.push_back(key); // apply operand ADT result = ADT::apply(f, op); diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index fe99ea975..331a76c13 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -56,7 +56,7 @@ bool Potentials::equals(const Potentials& other, double tol) const { /* ************************************************************************* */ void Potentials::print(const string& s, const KeyFormatter& formatter) const { cout << s << "\n Cardinalities: {"; - for (const DiscreteKey& key : cardinalities_) + for (const std::pair& key : cardinalities_) cout << formatter(key.first) << ":" << key.second << ", "; cout << "}" << endl; ADT::print(" "); diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 1c602a963..24a42afb8 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build( /* getting the block diagonals over the factors */ std::map hessianMap =gfg.hessianBlockDiagonal(); - for ( const Matrix hessian: hessianMap | boost::adaptors::map_values) + for (const Matrix& hessian: hessianMap | boost::adaptors::map_values) blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 0a25617e4..9b5868744 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -45,7 +45,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues::VectorValues(const Vector& x, const Dims& dims) { - typedef pair Pair; + using Pair = pair; size_t j = 0; for (const Pair& v : dims) { Key key; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4a95b4297..3a2d04127 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -73,8 +73,8 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name + ".xml"); // Find first name that exists - for(const fs::path& root: rootsToSearch) { - for(const fs::path& name: namesToSearch) { + for(const fs::path root: rootsToSearch) { + for(const fs::path name: namesToSearch) { if (fs::is_regular_file(root / name)) return (root / name).string(); } diff --git a/wrap/Module.cpp b/wrap/Module.cpp index ec02dc412..780c6f8da 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -248,7 +248,7 @@ void Module::parseMarkup(const std::string& data) { // Create type attributes table and check validity typeAttributes.addClasses(expandedClasses); typeAttributes.addForwardDeclarations(forward_declarations); - for (const TypedefPair p: typedefs) + for (const TypedefPair& p: typedefs) typeAttributes.addType(p.newType); // add Eigen types as template arguments are also checked ? vector eigen; From 8a9780113a855f4734365fce2a6b0ae969ff6fc4 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 27 Jul 2020 00:16:28 +0200 Subject: [PATCH 25/32] remove leftover cmake debug trace --- cmake/GtsamBuildTypes.cmake | 2 -- 1 file changed, 2 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 53b21647d..53dacd3f5 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -105,8 +105,6 @@ if(MSVC) else() # Common to all configurations, next for each configuration: - message (STATUS "CMAKE_CXX_COMPILER_VERSION: ${CMAKE_CXX_COMPILER_VERSION}") - if ( ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR (CMAKE_CXX_COMPILER_ID MATCHES "GNU") From ccbdb40c936cb3f7f12e5d14cd0fa328c09e9a0c Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 26 Jul 2020 20:08:55 -0400 Subject: [PATCH 26/32] fix warnings on incorrect for range reference bindings --- gtsam/linear/tests/testGaussianBayesNet.cpp | 8 ++++---- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 488368c72..eafefb3cb 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -137,7 +137,7 @@ TEST( GaussianBayesNet, optimize3 ) } /* ************************************************************************* */ -TEST(GaussianBayesNet, ordering) +TEST(GaussianBayesNet, ordering) { Ordering expected; expected += _x_, _y_; @@ -155,7 +155,7 @@ TEST( GaussianBayesNet, MatrixStress ) bn.emplace_shared(_z_, Vector2(5, 6), 6 * I_2x2); const VectorValues expected = bn.optimize(); - for (const auto keys : + for (const auto& keys : {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}), KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}), KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) { @@ -183,7 +183,7 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); - + const auto ordering = noisyBayesNet.ordering(); const Matrix R = smallBayesNet.matrix(ordering).first; const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); @@ -206,7 +206,7 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) VectorValues actual = noisyBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); - + const auto ordering = noisyBayesNet.ordering(); const Matrix R = noisyBayesNet.matrix(ordering).first; const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index b07b5acd6..d23346896 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -244,7 +244,7 @@ TEST(Similarity3, GroupAction) { &Similarity3::transformFrom, _1, _2, boost::none, boost::none); Point3 q(1, 2, 3); - for (const auto T : { T1, T2, T3, T4, T5, T6 }) { + for (const auto& T : { T1, T2, T3, T4, T5, T6 }) { Point3 q(1, 0, 0); Matrix H1 = numericalDerivative21(f, T, q); Matrix H2 = numericalDerivative22(f, T, q); From ced6eac7e2c2792b02c3ef55533b3210dfd2571b Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 26 Jul 2020 20:13:10 -0400 Subject: [PATCH 27/32] remove useless Planning test --- .../discrete/tests/testPlanning.cpp | 37 ------------------- 1 file changed, 37 deletions(-) delete mode 100644 gtsam_unstable/discrete/tests/testPlanning.cpp diff --git a/gtsam_unstable/discrete/tests/testPlanning.cpp b/gtsam_unstable/discrete/tests/testPlanning.cpp deleted file mode 100644 index 431d7a4ef..000000000 --- a/gtsam_unstable/discrete/tests/testPlanning.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* - * testPlanning.cpp - * @brief develop code for planning example - * @date Feb 13, 2012 - * @author Frank Dellaert - */ - -//#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST_UNSAFE(Planner, allInOne) -{ - // A small planning problem - // First variable is location - DiscreteKey location(0,3), - haveRock(1,2), haveSoil(2,2), haveImage(3,2), - commRock(4,2), commSoil(5,2), commImage(6,2); - - // There are 3 actions: - // Drive, communicate, sample -} - - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - From 1b775798100c55969bebac4f0d2fd99cf1e22da3 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 26 Jul 2020 20:16:32 -0400 Subject: [PATCH 28/32] fix return copy --- gtsam_unstable/discrete/tests/testScheduler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 0be4e4b1f..3f6c6a1e0 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -75,7 +75,7 @@ DiscreteFactorGraph createExpected() { // Mutual exclusion for students expected.addAllDiff(A, J); - return expected; + return std::move(expected); } /* ************************************************************************* */ From e9c7e2cb4f21ff61b51616cf74ba1c19d952f0ba Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 27 Jul 2020 22:11:50 -0400 Subject: [PATCH 29/32] add override keyword --- gtsam/navigation/MagFactor.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 97a4c70ce..74e9177d5 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -73,7 +73,7 @@ public: * @brief vector of errors */ Vector evaluateError(const Rot2& nRb, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { // measured bM = nRb� * nM + b Point3 hx = unrotate(nRb, nM_, H) + bias_; return (hx - measured_); @@ -111,7 +111,7 @@ public: * @brief vector of errors */ Vector evaluateError(const Rot3& nRb, - boost::optional H = boost::none) const { + boost::optional H = boost::none) const override { // measured bM = nRb� * nM + b Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; return (hx - measured_); @@ -150,7 +150,7 @@ public: */ Vector evaluateError(const Point3& nM, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { + boost::none) const override { // measured bM = nRb� * nM + b, where b is unknown bias Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) @@ -192,7 +192,7 @@ public: Vector evaluateError(const double& scale, const Unit3& direction, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = - boost::none) const { + boost::none) const override { // measured bM = nRb� * nM + b, where b is unknown bias Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Point3 hx = scale * rotated.point3() + bias; From e0791f4e54ea7845b634a2f0c96f2a08f8f712de Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 27 Jul 2020 23:43:35 -0400 Subject: [PATCH 30/32] workaround to dereferencing a nullptr --- gtsam/nonlinear/tests/testCallRecord.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index c5ccc0f52..66c56e696 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -98,7 +98,8 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -internal::JacobianMap & NJM= *static_cast(nullptr); +internal::JacobianMap* NJM_ptr = static_cast(nullptr); +internal::JacobianMap & NJM = *NJM_ptr; /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; From c9fb0960022569f35080f4fe8b43757b4f367b80 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 28 Jul 2020 00:19:44 +0200 Subject: [PATCH 31/32] Add genericValue() helper --- gtsam/base/GenericValue.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 98425adde..4b353e6de 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -198,4 +198,12 @@ const ValueType& Value::cast() const { return dynamic_cast&>(*this).value(); } +/** Functional constructor of GenericValue so T can be automatically deduced + */ +template +GenericValue genericValue(const T& v) { + return GenericValue(v); +} + + } /* namespace gtsam */ From 77c8c3bf0b095b37d84c41f7dc44a24156b90ae6 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 28 Jul 2020 00:22:09 +0200 Subject: [PATCH 32/32] Values initializer_list constructor --- gtsam/nonlinear/Values.cpp | 6 ++++++ gtsam/nonlinear/Values.h | 7 +++++++ gtsam/nonlinear/tests/testValues.cpp | 30 ++++++++++++++++++++++++++++ 3 files changed, 43 insertions(+) diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 7e13a072a..13be88ce1 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -52,6 +52,12 @@ namespace gtsam { Values::Values(Values&& other) : values_(std::move(other.values_)) { } + /* ************************************************************************* */ + Values::Values(std::initializer_list init) { + for (const auto &kv : init) + insert(kv.key, kv.value); + } + /* ************************************************************************* */ Values::Values(const Values& other, const VectorValues& delta) { for (const_iterator key_value = other.begin(); key_value != other.end(); ++key_value) { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bc64f2612..aadbcf394 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -149,6 +149,13 @@ namespace gtsam { /** Move constructor */ Values(Values&& other); + + /** Constructor from initializer list. Example usage: + * \code + * Values v = {{k1, genericValue(pose1)}, {k2, genericValue(point2)}}; + * \endcode + */ + Values(std::initializer_list init); /** Construct from a Values and an update vector: identical to other.retract(delta) */ Values(const Values& other, const VectorValues& delta); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 2f624f527..388bcf568 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -606,6 +606,36 @@ TEST(Values, Demangle) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(Values, brace_initializer) { + const Pose2 poseA(1.0, 2.0, 0.3), poseC(.0, .0, .0); + const Pose3 poseB(Pose2(0.1, 0.2, 0.3)); + + { + Values values; + EXPECT_LONGS_EQUAL(0, values.size()); + values = { {key1, genericValue(1.0)} }; + EXPECT_LONGS_EQUAL(1, values.size()); + CHECK(values.at(key1) == 1.0); + } + { + Values values = { {key1, genericValue(poseA)}, {key2, genericValue(poseB)} }; + EXPECT_LONGS_EQUAL(2, values.size()); + EXPECT(assert_equal(values.at(key1), poseA)); + EXPECT(assert_equal(values.at(key2), poseB)); + } + // Test exception: duplicated key: + { + Values values; + CHECK_EXCEPTION((values = { + {key1, genericValue(poseA)}, + {key2, genericValue(poseB)}, + {key1, genericValue(poseC)} + }), std::exception); + } +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */