Fixed warnings that arise from stricter compiler flags

release/4.3a0
Frank Dellaert 2023-02-11 12:32:50 -08:00
parent b71ed8f91d
commit a8eb98acea
64 changed files with 126 additions and 181 deletions

View File

@ -32,7 +32,7 @@ class Test
public: public:
Test (const std::string& testName); Test (const std::string& testName);
Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck); Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck);
virtual ~Test() {}; virtual ~Test() {}
virtual void run (TestResult& result) = 0; virtual void run (TestResult& result) = 0;
@ -63,7 +63,6 @@ protected:
#define TEST(testGroup, testName)\ #define TEST(testGroup, testName)\
class testGroup##testName##Test : public Test \ class testGroup##testName##Test : public Test \
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
virtual ~testGroup##testName##Test () {};\
void run (TestResult& result_) override;} \ void run (TestResult& result_) override;} \
testGroup##testName##Instance; \ testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_) void testGroup##testName##Test::run (TestResult& result_)

View File

@ -27,7 +27,7 @@ class TestResult
{ {
public: public:
TestResult (); TestResult ();
virtual ~TestResult() {}; virtual ~TestResult() {}
virtual void testsStarted (); virtual void testsStarted ();
virtual void addFailure (const Failure& failure); virtual void addFailure (const Failure& failure);
virtual void testsEnded (); virtual void testsEnded ();

View File

@ -130,6 +130,8 @@ else()
$<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wno-misleading-indentation> # Eigen triggers a ton! $<$<CXX_COMPILER_ID:Clang>:-Wno-misleading-indentation> # Eigen triggers a ton!
$<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve
$<$<CXX_COMPILER_ID:Clang>:-Wno-weak-vtables> # TODO(dellaert): don't know how to resolve
-Wreturn-type -Werror=return-type # Error on missing return() -Wreturn-type -Werror=return-type # Error on missing return()
-Wformat -Werror=format-security # Error on wrong printf() arguments -Wformat -Werror=format-security # Error on wrong printf() arguments
$<$<COMPILE_LANGUAGE:CXX>:${flag_override_}> # Enforce the use of the override keyword $<$<COMPILE_LANGUAGE:CXX>:${flag_override_}> # Enforce the use of the override keyword

View File

@ -118,8 +118,8 @@ class IndexPair : public std::pair<size_t,size_t> {
public: public:
inline IndexPair(): std::pair<size_t,size_t>(0,0) {} inline IndexPair(): std::pair<size_t,size_t>(0,0) {}
inline IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {} inline IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
inline size_t i() const { return first; }; inline size_t i() const { return first; }
inline size_t j() const { return second; }; inline size_t j() const { return second; }
}; };
typedef std::vector<IndexPair> IndexPairVector; typedef std::vector<IndexPair> IndexPairVector;

View File

@ -23,11 +23,11 @@
// These do something sensible: // These do something sensible:
#define BOOST_CONCEPT_USAGE(concept) void check##concept() #define BOOST_CONCEPT_USAGE(concept) void check##concept()
// TODO(dellaert): would be nice if it was a single macro... // TODO(dellaert): would be nice if it was a single macro...
#define GTSAM_CONCEPT_ASSERT(concept) concept checkConcept [[maybe_unused]]; #define GTSAM_CONCEPT_ASSERT(concept) concept checkConcept [[maybe_unused]]
#define GTSAM_CONCEPT_ASSERT1(concept) concept checkConcept1 [[maybe_unused]]; #define GTSAM_CONCEPT_ASSERT1(concept) concept checkConcept1 [[maybe_unused]]
#define GTSAM_CONCEPT_ASSERT2(concept) concept checkConcept2 [[maybe_unused]]; #define GTSAM_CONCEPT_ASSERT2(concept) concept checkConcept2 [[maybe_unused]]
#define GTSAM_CONCEPT_ASSERT3(concept) concept checkConcept3 [[maybe_unused]]; #define GTSAM_CONCEPT_ASSERT3(concept) concept checkConcept3 [[maybe_unused]]
#define GTSAM_CONCEPT_ASSERT4(concept) concept checkConcept4 [[maybe_unused]]; #define GTSAM_CONCEPT_ASSERT4(concept) concept checkConcept4 [[maybe_unused]]
// This one just ignores concept for now: // This one just ignores concept for now:
#define GTSAM_CONCEPT_REQUIRES(concept, return_type) return_type #define GTSAM_CONCEPT_REQUIRES(concept, return_type) return_type
#endif #endif

View File

@ -42,7 +42,7 @@ std::string demangle(const char* name) {
// g++ version of demangle // g++ version of demangle
char* demangled = nullptr; char* demangled = nullptr;
int status = -1; // some arbitrary value to eliminate the compiler warning int status = -1; // some arbitrary value to eliminate the compiler warning
demangled = abi::__cxa_demangle(name, nullptr, nullptr, &status), demangled = abi::__cxa_demangle(name, nullptr, nullptr, &status);
demangled_name = (status == 0) ? std::string(demangled) : std::string(name); demangled_name = (status == 0) ? std::string(demangled) : std::string(name);

View File

@ -65,9 +65,6 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph)
: Base(graph) {} : Base(graph) {}
/// Destructor
virtual ~DiscreteBayesNet() {}
/// @} /// @}
/// @name Testable /// @name Testable

View File

@ -45,7 +45,6 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
typedef std::shared_ptr<This> shared_ptr; typedef std::shared_ptr<This> shared_ptr;
typedef std::weak_ptr<This> weak_ptr; typedef std::weak_ptr<This> weak_ptr;
DiscreteBayesTreeClique() {} DiscreteBayesTreeClique() {}
virtual ~DiscreteBayesTreeClique() {}
DiscreteBayesTreeClique( DiscreteBayesTreeClique(
const std::shared_ptr<DiscreteConditional>& conditional) const std::shared_ptr<DiscreteConditional>& conditional)
: Base(conditional) {} : Base(conditional) {}

View File

@ -59,10 +59,6 @@ public:
template<typename CONTAINER> template<typename CONTAINER>
DiscreteFactor(const CONTAINER& keys) : Base(keys) {} DiscreteFactor(const CONTAINER& keys) : Base(keys) {}
/// Virtual destructor
virtual ~DiscreteFactor() {
}
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{

View File

@ -111,9 +111,6 @@ class GTSAM_EXPORT DiscreteFactorGraph
template <class DERIVEDFACTOR> template <class DERIVEDFACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// Destructor
virtual ~DiscreteFactorGraph() {}
/// @name Testable /// @name Testable
/// @{ /// @{

View File

@ -89,9 +89,6 @@ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet<DiscreteLookupTable> {
/// Create from BayesNet with LookupTables /// Create from BayesNet with LookupTables
static DiscreteLookupDAG FromBayesNet(const DiscreteBayesNet& bayesNet); static DiscreteLookupDAG FromBayesNet(const DiscreteBayesNet& bayesNet);
/// Destructor
virtual ~DiscreteLookupDAG() {}
/// @} /// @}
/// @name Testable /// @name Testable

View File

@ -38,7 +38,7 @@ std::optional<Row> static ParseConditional(const std::string& token) {
} catch (...) { } catch (...) {
return std::nullopt; return std::nullopt;
} }
return row; return std::move(row);
} }
std::optional<Table> static ParseConditionalTable( std::optional<Table> static ParseConditionalTable(
@ -62,7 +62,7 @@ std::optional<Table> static ParseConditionalTable(
} }
} }
} }
return table; return std::move(table);
} }
std::vector<std::string> static Tokenize(const std::string& str) { std::vector<std::string> static Tokenize(const std::string& str) {

View File

@ -113,7 +113,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
// Set px and py using intrinsic coordinates since that is where radial // Set px and py using intrinsic coordinates since that is where radial
// distortion correction is done. // distortion correction is done.
px = pn.x(), py = pn.y(); px = pn.x();
py = pn.y();
iteration++; iteration++;
} while (iteration < maxIterations); } while (iteration < maxIterations);

View File

@ -31,7 +31,7 @@ std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const { Base::print(s_); } void Cal3DS2::print(const std::string& s) const { Base::print(s); }
/* ************************************************************************* */ /* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {

View File

@ -40,9 +40,9 @@ std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const { void Cal3DS2_Base::print(const std::string& s) const {
gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print((Matrix)K(), s + ".K");
gtsam::print(Vector(k()), s_ + ".k"); gtsam::print(Vector(k()), s + ".k");
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -162,9 +162,9 @@ std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3Fisheye::print(const std::string& s_) const { void Cal3Fisheye::print(const std::string& s) const {
gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print((Matrix)K(), s + ".K");
gtsam::print(Vector(k()), s_ + ".k"); gtsam::print(Vector(k()), s + ".k");
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -308,14 +308,6 @@ public:
PinholeBase(v) { PinholeBase(v) {
} }
/// @}
/// @name Standard Interface
/// @{
/// destructor
virtual ~CalibratedCamera() {
}
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{

View File

@ -321,7 +321,7 @@ public:
/// for Nonlinear Triangulation /// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const { Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());; return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());
} }
private: private:

View File

@ -67,7 +67,7 @@ public:
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
virtual ~PinholeBaseK() { virtual ~PinholeBaseK() override {
} }
/// return calibration /// return calibration
@ -425,7 +425,7 @@ public:
/// for Nonlinear Triangulation /// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const { Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());; return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());
} }
/// @} /// @}

View File

@ -172,7 +172,7 @@ public:
/// for Nonlinear Triangulation /// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const { Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());; return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());
} }
/// @} /// @}

View File

@ -55,8 +55,6 @@ class GTSAM_EXPORT HybridBayesTreeClique
: Base(conditional) {} : Base(conditional) {}
///< Copy constructor ///< Copy constructor
HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {} HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {}
virtual ~HybridBayesTreeClique() {}
}; };
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -97,9 +97,6 @@ class GTSAM_EXPORT HybridFactor : public Factor {
HybridFactor(const KeyVector &continuousKeys, HybridFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys); const DiscreteKeys &discreteKeys);
/// Virtual destructor
virtual ~HybridFactor() = default;
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{

View File

@ -287,7 +287,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
auto hf = std::dynamic_pointer_cast<HessianFactor>(factor); auto hf = std::dynamic_pointer_cast<HessianFactor>(factor);
if (!hf) throw std::runtime_error("Expected HessianFactor!"); if (!hf) throw std::runtime_error("Expected HessianFactor!");
hf->constantTerm() += 2.0 * pair.first->logNormalizationConstant(); hf->constantTerm() += 2.0 * pair.first->logNormalizationConstant();
return hf; return std::move(hf);
}; };
GaussianMixtureFactor::Factors correctedFactors(eliminationResults, GaussianMixtureFactor::Factors correctedFactors(eliminationResults,

View File

@ -54,13 +54,13 @@ class GTSAM_EXPORT HybridValues {
HybridValues() = default; HybridValues() = default;
/// Construct from DiscreteValues and VectorValues. /// Construct from DiscreteValues and VectorValues.
HybridValues(const VectorValues& cv, const DiscreteValues& dv) HybridValues(const VectorValues &cv, const DiscreteValues &dv)
: continuous_(cv), discrete_(dv){}; : continuous_(cv), discrete_(dv){}
/// Construct from all values types. /// Construct from all values types.
HybridValues(const VectorValues& cv, const DiscreteValues& dv, HybridValues(const VectorValues& cv, const DiscreteValues& dv,
const Values& v) const Values& v)
: continuous_(cv), discrete_(dv), nonlinear_(v){}; : continuous_(cv), discrete_(dv), nonlinear_(v){}
/// @} /// @}
/// @name Testable /// @name Testable
@ -73,7 +73,7 @@ class GTSAM_EXPORT HybridValues {
continuous_.print(" Continuous", continuous_.print(" Continuous",
keyFormatter); // print continuous components keyFormatter); // print continuous components
discrete_.print(" Discrete", keyFormatter); // print discrete components discrete_.print(" Discrete", keyFormatter); // print discrete components
}; }
/// equals required by Testable for unit testing /// equals required by Testable for unit testing
bool equals(const HybridValues& other, double tol = 1e-9) const { bool equals(const HybridValues& other, double tol = 1e-9) const {
@ -95,20 +95,20 @@ class GTSAM_EXPORT HybridValues {
const Values& nonlinear() const { return nonlinear_; } const Values& nonlinear() const { return nonlinear_; }
/// Check whether a variable with key \c j exists in VectorValues. /// Check whether a variable with key \c j exists in VectorValues.
bool existsVector(Key j) { return continuous_.exists(j); }; bool existsVector(Key j) { return continuous_.exists(j); }
/// Check whether a variable with key \c j exists in DiscreteValues. /// Check whether a variable with key \c j exists in DiscreteValues.
bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }; bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }
/// Check whether a variable with key \c j exists in values. /// Check whether a variable with key \c j exists in values.
bool existsNonlinear(Key j) { bool existsNonlinear(Key j) {
return nonlinear_.exists(j); return nonlinear_.exists(j);
}; }
/// Check whether a variable with key \c j exists. /// Check whether a variable with key \c j exists.
bool exists(Key j) { bool exists(Key j) {
return existsVector(j) || existsDiscrete(j) || existsNonlinear(j); return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
}; }
/** Insert a vector \c value with key \c j. Throws an invalid_argument /** Insert a vector \c value with key \c j. Throws an invalid_argument
* exception if the key \c j is already used. * exception if the key \c j is already used.
@ -120,7 +120,7 @@ class GTSAM_EXPORT HybridValues {
* the key \c j is already used. * the key \c j is already used.
* @param value The vector to be inserted. * @param value The vector to be inserted.
* @param j The index with which the value will be associated. */ * @param j The index with which the value will be associated. */
void insert(Key j, size_t value) { discrete_[j] = value; }; void insert(Key j, size_t value) { discrete_[j] = value; }
/// insert_or_assign() , similar to Values.h /// insert_or_assign() , similar to Values.h
void insert_or_assign(Key j, const Vector& value) { void insert_or_assign(Key j, const Vector& value) {
@ -166,13 +166,13 @@ class GTSAM_EXPORT HybridValues {
* Read/write access to the vector value with key \c j, throws * Read/write access to the vector value with key \c j, throws
* std::out_of_range if \c j does not exist. * std::out_of_range if \c j does not exist.
*/ */
Vector& at(Key j) { return continuous_.at(j); }; Vector& at(Key j) { return continuous_.at(j); }
/** /**
* Read/write access to the discrete value with key \c j, throws * Read/write access to the discrete value with key \c j, throws
* std::out_of_range if \c j does not exist. * std::out_of_range if \c j does not exist.
*/ */
size_t& atDiscrete(Key j) { return discrete_.at(j); }; size_t& atDiscrete(Key j) { return discrete_.at(j); }
/** For all key/value pairs in \c values, replace continuous values with /** For all key/value pairs in \c values, replace continuous values with
* corresponding keys in this object with those in \c values. Throws * corresponding keys in this object with those in \c values. Throws
@ -227,7 +227,7 @@ class GTSAM_EXPORT HybridValues {
ss << this->continuous_.html(keyFormatter); ss << this->continuous_.html(keyFormatter);
ss << this->discrete_.html(keyFormatter); ss << this->discrete_.html(keyFormatter);
return ss.str(); return ss.str();
}; }
/// @} /// @}
}; };

View File

@ -123,8 +123,6 @@ class MixtureFactor : public HybridFactor {
} }
} }
~MixtureFactor() = default;
/** /**
* @brief Compute error of the MixtureFactor as a tree. * @brief Compute error of the MixtureFactor as a tree.
* *

View File

@ -18,6 +18,7 @@
#include <gtsam/inference/LabeledSymbol.h> #include <gtsam/inference/LabeledSymbol.h>
#include <iostream> #include <iostream>
#include <limits>
namespace gtsam { namespace gtsam {

View File

@ -85,7 +85,7 @@ public:
operator std::string() const; operator std::string() const;
/// Return string representation of the key /// Return string representation of the key
std::string string() const { return std::string(*this); }; std::string string() const { return std::string(*this); }
/** Comparison for use in maps */ /** Comparison for use in maps */
bool operator<(const Symbol& comp) const { bool operator<(const Symbol& comp) const {

View File

@ -18,6 +18,7 @@
#include <gtsam/inference/VariableSlots.h> #include <gtsam/inference/VariableSlots.h>
#include <iostream> #include <iostream>
#include <limits>
using namespace std; using namespace std;

View File

@ -74,9 +74,6 @@ namespace gtsam {
std::initializer_list<std::shared_ptr<DERIVEDCONDITIONAL> > conditionals) std::initializer_list<std::shared_ptr<DERIVEDCONDITIONAL> > conditionals)
: Base(conditionals) {} : Base(conditionals) {}
/// Destructor
virtual ~GaussianBayesNet() = default;
/// @} /// @}
/// @name Testable /// @name Testable

View File

@ -41,7 +41,6 @@ namespace gtsam {
typedef std::shared_ptr<This> shared_ptr; typedef std::shared_ptr<This> shared_ptr;
typedef std::weak_ptr<This> weak_ptr; typedef std::weak_ptr<This> weak_ptr;
GaussianBayesTreeClique() {} GaussianBayesTreeClique() {}
virtual ~GaussianBayesTreeClique() {}
GaussianBayesTreeClique(const std::shared_ptr<GaussianConditional>& conditional) : Base(conditional) {} GaussianBayesTreeClique(const std::shared_ptr<GaussianConditional>& conditional) : Base(conditional) {}
}; };

View File

@ -51,7 +51,7 @@ namespace gtsam {
GaussianFactor(const CONTAINER& keys) : Base(keys) {} GaussianFactor(const CONTAINER& keys) : Base(keys) {}
/** Destructor */ /** Destructor */
virtual ~GaussianFactor() {} virtual ~GaussianFactor() override {}
// Implementing Testable interface // Implementing Testable interface

View File

@ -249,7 +249,7 @@ namespace gtsam {
// combine all factors and get upper-triangular part of Hessian // combine all factors and get upper-triangular part of Hessian
Scatter scatter(*this, ordering); Scatter scatter(*this, ordering);
HessianFactor combined(*this, scatter); HessianFactor combined(*this, scatter);
return combined.info().selfadjointView();; return combined.info().selfadjointView();
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -257,7 +257,7 @@ namespace gtsam {
// combine all factors and get upper-triangular part of Hessian // combine all factors and get upper-triangular part of Hessian
Scatter scatter(*this); Scatter scatter(*this);
HessianFactor combined(*this, scatter); HessianFactor combined(*this, scatter);
return combined.info().selfadjointView();; return combined.info().selfadjointView();
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -108,7 +108,7 @@ namespace gtsam {
GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/** Virtual destructor */ /** Virtual destructor */
virtual ~GaussianFactorGraph() {} virtual ~GaussianFactorGraph() override {}
/// @} /// @}
/// @name Testable /// @name Testable

View File

@ -378,7 +378,7 @@ GaussianFactor::shared_ptr HessianFactor::negate() const {
shared_ptr result = std::make_shared<This>(*this); shared_ptr result = std::make_shared<This>(*this);
// Negate the information matrix of the result // Negate the information matrix of the result
result->info_.negate(); result->info_.negate();
return result; return std::move(result);
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -63,7 +63,7 @@ std::optional<Vector> checkIfDiagonal(const Matrix& M) {
Vector diagonal(n); Vector diagonal(n);
for (j = 0; j < n; j++) for (j = 0; j < n; j++)
diagonal(j) = M(j, j); diagonal(j) = M(j, j);
return diagonal; return std::move(diagonal);
} }
} }

View File

@ -668,7 +668,7 @@ namespace gtsam {
public: public:
/// Default Constructor for serialization /// Default Constructor for serialization
Robust() {}; Robust() {}
/// Constructor /// Constructor
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise) Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)

View File

@ -48,7 +48,7 @@ void PCGSolverParameters::setPreconditionerParams(const std::shared_ptr<Precondi
} }
void PCGSolverParameters::print(const std::string &s) const { void PCGSolverParameters::print(const std::string &s) const {
std::cout << s << std::endl;; std::cout << s << std::endl;
std::ostringstream os; std::ostringstream os;
print(os); print(os);
std::cout << os.str() << std::endl; std::cout << os.str() << std::endl;

View File

@ -18,7 +18,12 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
using namespace std; #include <functional>
#include <utility>
// assert_throw needs a semicolon in Release mode.
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wextra-semi-stmt"
namespace gtsam { namespace gtsam {
@ -27,10 +32,10 @@ namespace gtsam {
{ {
// Merge using predicate for comparing first of pair // Merge using predicate for comparing first of pair
merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()),
std::bind(&less<Key>::operator(), less<Key>(), std::bind(&KeyValuePair::first, std::placeholders::_1), std::bind(&std::less<Key>::operator(), std::less<Key>(), std::bind(&KeyValuePair::first, std::placeholders::_1),
std::bind(&KeyValuePair::first, std::placeholders::_2))); std::bind(&KeyValuePair::first, std::placeholders::_2)));
if(size() != first.size() + second.size()) if(size() != first.size() + second.size())
throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); throw std::invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
} }
/* ************************************************************************ */ /* ************************************************************************ */
@ -92,7 +97,7 @@ namespace gtsam {
hint = values_.insert(hint, {key, value}); hint = values_.insert(hint, {key, value});
if (values_.size() > oldSize) { if (values_.size() > oldSize) {
values_.unsafe_erase(hint); values_.unsafe_erase(hint);
throw out_of_range( throw std::out_of_range(
"Requested to update a VectorValues with another VectorValues that " "Requested to update a VectorValues with another VectorValues that "
"contains keys not present in the first."); "contains keys not present in the first.");
} else { } else {
@ -107,7 +112,7 @@ namespace gtsam {
size_t originalSize = size(); size_t originalSize = size();
values_.insert(values.begin(), values.end()); values_.insert(values.begin(), values.end());
if (size() != originalSize + values.size()) if (size() != originalSize + values.size())
throw invalid_argument( throw std::invalid_argument(
"Requested to insert a VectorValues into another VectorValues that " "Requested to insert a VectorValues into another VectorValues that "
"already contains one or more of its keys."); "already contains one or more of its keys.");
return *this; return *this;
@ -122,7 +127,7 @@ namespace gtsam {
} }
/* ************************************************************************ */ /* ************************************************************************ */
GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) { GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) {
// Change print depending on whether we are using TBB // Change print depending on whether we are using TBB
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
map<Key, Vector> sorted; map<Key, Vector> sorted;
@ -140,11 +145,11 @@ namespace gtsam {
} }
/* ************************************************************************ */ /* ************************************************************************ */
void VectorValues::print(const string& str, void VectorValues::print(const std::string& str,
const KeyFormatter& formatter) const { const KeyFormatter& formatter) const {
cout << str << ": " << size() << " elements\n"; std::cout << str << ": " << size() << " elements\n";
cout << key_formatter(formatter) << *this; std::cout << key_formatter(formatter) << *this;
cout.flush(); std::cout.flush();
} }
/* ************************************************************************ */ /* ************************************************************************ */
@ -222,15 +227,15 @@ namespace gtsam {
double VectorValues::dot(const VectorValues& v) const double VectorValues::dot(const VectorValues& v) const
{ {
if(this->size() != v.size()) if(this->size() != v.size())
throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); throw std::invalid_argument("VectorValues::dot called with a VectorValues of different structure");
double result = 0.0; double result = 0.0;
auto this_it = this->begin(); auto this_it = this->begin();
auto v_it = v.begin(); auto v_it = v.begin();
for(; this_it != this->end(); ++this_it, ++v_it) { for(; this_it != this->end(); ++this_it, ++v_it) {
assert_throw(this_it->first == v_it->first, assert_throw(this_it->first == v_it->first,
invalid_argument("VectorValues::dot called with a VectorValues of different structure")); std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
assert_throw(this_it->second.size() == v_it->second.size(), assert_throw(this_it->second.size() == v_it->second.size(),
invalid_argument("VectorValues::dot called with a VectorValues of different structure")); std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
result += this_it->second.dot(v_it->second); result += this_it->second.dot(v_it->second);
} }
return result; return result;
@ -254,9 +259,9 @@ namespace gtsam {
VectorValues VectorValues::operator+(const VectorValues& c) const VectorValues VectorValues::operator+(const VectorValues& c) const
{ {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes"); throw std::invalid_argument("VectorValues::operator+ called with different vector sizes");
assert_throw(hasSameStructure(c), assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator+ called with different vector sizes")); std::invalid_argument("VectorValues::operator+ called with different vector sizes"));
VectorValues result; VectorValues result;
// The result.end() hint here should result in constant-time inserts // The result.end() hint here should result in constant-time inserts
@ -280,9 +285,9 @@ namespace gtsam {
VectorValues& VectorValues::operator+=(const VectorValues& c) VectorValues& VectorValues::operator+=(const VectorValues& c)
{ {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes"); throw std::invalid_argument("VectorValues::operator+= called with different vector sizes");
assert_throw(hasSameStructure(c), assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator+= called with different vector sizes")); std::invalid_argument("VectorValues::operator+= called with different vector sizes"));
iterator j1 = begin(); iterator j1 = begin();
const_iterator j2 = c.begin(); const_iterator j2 = c.begin();
@ -303,11 +308,11 @@ namespace gtsam {
VectorValues& VectorValues::addInPlace_(const VectorValues& c) VectorValues& VectorValues::addInPlace_(const VectorValues& c)
{ {
for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) { for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
pair<VectorValues::iterator, bool> xi = tryInsert(j2->first, Vector()); const auto& [it, success] = tryInsert(j2->first, Vector());
if(xi.second) if(success)
xi.first->second = j2->second; it->second = j2->second;
else else
xi.first->second += j2->second; it->second += j2->second;
} }
return *this; return *this;
} }
@ -316,9 +321,9 @@ namespace gtsam {
VectorValues VectorValues::operator-(const VectorValues& c) const VectorValues VectorValues::operator-(const VectorValues& c) const
{ {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes"); throw std::invalid_argument("VectorValues::operator- called with different vector sizes");
assert_throw(hasSameStructure(c), assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator- called with different vector sizes")); std::invalid_argument("VectorValues::operator- called with different vector sizes"));
VectorValues result; VectorValues result;
// The result.end() hint here should result in constant-time inserts // The result.end() hint here should result in constant-time inserts
@ -373,8 +378,8 @@ namespace gtsam {
} }
/* ************************************************************************ */ /* ************************************************************************ */
string VectorValues::html(const KeyFormatter& keyFormatter) const { std::string VectorValues::html(const KeyFormatter& keyFormatter) const {
stringstream ss; std::stringstream ss;
// Print out preamble. // Print out preamble.
ss << "<div>\n<table class='VectorValues'>\n <thead>\n"; ss << "<div>\n<table class='VectorValues'>\n <thead>\n";
@ -406,3 +411,6 @@ namespace gtsam {
/* ************************************************************************ */ /* ************************************************************************ */
} // \namespace gtsam } // \namespace gtsam
#pragma clang diagnostic pop

View File

@ -41,23 +41,23 @@ more information.");
/* ************************************************************************* */ /* ************************************************************************* */
const char* InvalidNoiseModel::what() const noexcept { const char* InvalidNoiseModel::what() const noexcept {
if(description_.empty()) if(description_->empty())
description_ = "A JacobianFactor was attempted to be constructed or modified to use a\n" description_ = "A JacobianFactor was attempted to be constructed or modified to use a\n"
"noise model of incompatible dimension. The JacobianFactor has\n" "noise model of incompatible dimension. The JacobianFactor has\n"
"dimensionality (i.e. length of error vector) " + std::to_string(factorDims) + "dimensionality (i.e. length of error vector) " + std::to_string(factorDims) +
" but the provided noise model has dimensionality " + std::to_string(noiseModelDims) + "."; " but the provided noise model has dimensionality " + std::to_string(noiseModelDims) + ".";
return description_.c_str(); return description_->c_str();
} }
/* ************************************************************************* */ /* ************************************************************************* */
const char* InvalidMatrixBlock::what() const noexcept { const char* InvalidMatrixBlock::what() const noexcept {
if(description_.empty()) { if(description_->empty()) {
description_ = "A JacobianFactor was attempted to be constructed with a matrix block of\n" description_ = "A JacobianFactor was attempted to be constructed with a matrix block of\n"
"inconsistent dimension. The JacobianFactor has " + std::to_string(factorRows) + "inconsistent dimension. The JacobianFactor has " + std::to_string(factorRows) +
" rows (i.e. length of error vector) but the provided matrix block has " + " rows (i.e. length of error vector) but the provided matrix block has " +
std::to_string(blockRows) + " rows."; std::to_string(blockRows) + " rows.";
} }
return description_.c_str(); return description_->c_str();
} }
} }

View File

@ -113,9 +113,6 @@ namespace gtsam {
~InvalidNoiseModel() noexcept override {} ~InvalidNoiseModel() noexcept override {}
const char* what() const noexcept override; const char* what() const noexcept override;
private:
mutable std::string description_;
}; };
/* ************************************************************************* */ /* ************************************************************************* */
@ -131,9 +128,6 @@ namespace gtsam {
~InvalidMatrixBlock() noexcept override {} ~InvalidMatrixBlock() noexcept override {}
const char* what() const noexcept override; const char* what() const noexcept override;
private:
mutable std::string description_;
}; };
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -89,12 +89,12 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
// From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html // From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) / return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) /
-0.00649; -0.00649;
}; }
inline double baroOut(const double& meters) { inline double baroOut(const double& meters) {
double temp = 15.04 - 0.00649 * meters; double temp = 15.04 - 0.00649 * meters;
return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
}; }
private: private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ///

View File

@ -71,8 +71,8 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
biasAccOmegaInt(I_6x6) {} biasAccOmegaInt(I_6x6) {}
/// See two named constructors below for good values of n_gravity in body frame /// See two named constructors below for good values of n_gravity in body frame
PreintegrationCombinedParams(const Vector3& n_gravity) : PreintegrationCombinedParams(const Vector3& _n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), PreintegrationParams(_n_gravity), biasAccCovariance(I_3x3),
biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) { biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
} }

View File

@ -18,12 +18,10 @@
#include <gtsam/navigation/NavState.h> #include <gtsam/navigation/NavState.h>
using namespace std; #include <string>
namespace gtsam { namespace gtsam {
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v, NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
@ -87,7 +85,7 @@ Matrix7 NavState::matrix() const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ostream& operator<<(ostream& os, const NavState& state) { std::ostream& operator<<(std::ostream& os, const NavState& state) {
os << "R: " << state.attitude() << "\n"; os << "R: " << state.attitude() << "\n";
os << "p: " << state.position().transpose() << "\n"; os << "p: " << state.position().transpose() << "\n";
os << "v: " << state.velocity().transpose(); os << "v: " << state.velocity().transpose();
@ -95,8 +93,8 @@ ostream& operator<<(ostream& os, const NavState& state) {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void NavState::print(const string& s) const { void NavState::print(const std::string& s) const {
cout << (s.empty() ? s : s + " ") << *this << endl; std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -108,7 +106,7 @@ bool NavState::equals(const NavState& other, double tol) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::retract(const Vector9& xi, // NavState NavState::retract(const Vector9& xi, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
TIE(nRb, n_t, n_v, *this); auto [nRb, n_t, n_v] = (*this);
Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
@ -214,7 +212,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
OptionalJacobian<9, 9> H) const { OptionalJacobian<9, 9> H) const {
TIE(nRb, n_t, n_v, *this); auto [nRb, n_t, n_v] = (*this);
const double dt2 = dt * dt; const double dt2 = dt * dt;
const Vector3 omega_cross_vel = omega.cross(n_v); const Vector3 omega_cross_vel = omega.cross(n_v);

View File

@ -69,8 +69,6 @@ public:
this->error_function_ = errorFunction; this->error_function_ = errorFunction;
} }
~CustomFactor() override = default;
/** /**
* Calls the errorFunction closure, which is a std::function object * Calls the errorFunction closure, which is a std::function object
* One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array

View File

@ -54,8 +54,8 @@ namespace internal {
struct DoglegState : public NonlinearOptimizerState { struct DoglegState : public NonlinearOptimizerState {
const double delta; const double delta;
DoglegState(const Values& values, double error, double delta, unsigned int iterations = 0) DoglegState(const Values& _values, double _error, double _delta, unsigned int _iterations = 0)
: NonlinearOptimizerState(values, error, iterations), delta(delta) {} : NonlinearOptimizerState(_values, _error, _iterations), delta(_delta) {}
}; };
} }

View File

@ -51,7 +51,6 @@ class GTSAM_EXPORT ISAM2Clique
/// Default constructor /// Default constructor
ISAM2Clique() : Base() {} ISAM2Clique() : Base() {}
virtual ~ISAM2Clique() = default;
/// Copy constructor, does *not* copy solution pointers as these are invalid /// Copy constructor, does *not* copy solution pointers as these are invalid
/// in different trees. /// in different trees.

View File

@ -145,7 +145,8 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
double modelFidelity = 0.0; double modelFidelity = 0.0;
bool step_is_successful = false; bool step_is_successful = false;
bool stopSearchingLambda = false; bool stopSearchingLambda = false;
double newError = numeric_limits<double>::infinity(), costChange; double newError = numeric_limits<double>::infinity();
double costChange = 0.0;
Values newValues; Values newValues;
VectorValues delta; VectorValues delta;

View File

@ -117,8 +117,8 @@ public:
} }
static LevenbergMarquardtParams ReplaceOrdering(LevenbergMarquardtParams params, static LevenbergMarquardtParams ReplaceOrdering(LevenbergMarquardtParams params,
const Ordering& ordering) { const Ordering& ord) {
params.ordering = ordering; params.ordering = ord;
return params; return params;
} }

View File

@ -106,7 +106,7 @@ public:
/// @{ /// @{
/** Destructor */ /** Destructor */
virtual ~NonlinearFactor() {} virtual ~NonlinearFactor() override {}
/** /**
* In nonlinear factors, the error function returns the negative log-likelihood * In nonlinear factors, the error function returns the negative log-likelihood

View File

@ -79,7 +79,7 @@ namespace gtsam {
NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// Destructor /// Destructor
virtual ~NonlinearFactorGraph() {} virtual ~NonlinearFactorGraph() override {}
/// @} /// @}
/// @name Testable /// @name Testable

View File

@ -49,19 +49,19 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
// optimization (for each iteration, LM tries multiple // optimization (for each iteration, LM tries multiple
// inner iterations with different lambdas) // inner iterations with different lambdas)
LevenbergMarquardtState(const Values& initialValues, double error, double lambda, LevenbergMarquardtState(const Values& initialValues, double _error, double _lambda,
double currentFactor, unsigned int iterations = 0, double currentFactor, unsigned int _iterations = 0,
unsigned int totalNumberInnerIterations = 0) unsigned int totalNumberInnerIterations = 0)
: NonlinearOptimizerState(initialValues, error, iterations), : NonlinearOptimizerState(initialValues, _error, _iterations),
lambda(lambda), lambda(_lambda),
currentFactor(currentFactor), currentFactor(currentFactor),
totalNumberInnerIterations(totalNumberInnerIterations) {} totalNumberInnerIterations(totalNumberInnerIterations) {}
// Constructor version that takes ownership of values // Constructor version that takes ownership of values
LevenbergMarquardtState(Values&& initialValues, double error, double lambda, double currentFactor, LevenbergMarquardtState(Values&& initialValues, double _error, double _lambda, double currentFactor,
unsigned int iterations = 0, unsigned int totalNumberInnerIterations = 0) unsigned int _iterations = 0, unsigned int totalNumberInnerIterations = 0)
: NonlinearOptimizerState(initialValues, error, iterations), : NonlinearOptimizerState(initialValues, _error, _iterations),
lambda(lambda), lambda(_lambda),
currentFactor(currentFactor), currentFactor(currentFactor),
totalNumberInnerIterations(totalNumberInnerIterations) {} totalNumberInnerIterations(totalNumberInnerIterations) {}
@ -119,7 +119,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
if (!item->model) if (!item->model)
*item = CachedModel(dim, 1.0 / std::sqrt(lambda)); *item = CachedModel(dim, 1.0 / std::sqrt(lambda));
return item; return item;
}; }
/// Build a damped system for a specific lambda, vanilla version /// Build a damped system for a specific lambda, vanilla version
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped /* gets copied */) const { GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped /* gets copied */) const {

View File

@ -52,9 +52,6 @@ private:
measured_(measured), measured_(measured),
noiseModel_(model) {} noiseModel_(model) {}
/// Destructor
virtual ~BinaryMeasurement() {}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{

View File

@ -48,8 +48,8 @@ struct Keypoints {
/// Optional confidences/responses for each detection, of shape N. /// Optional confidences/responses for each detection, of shape N.
std::optional<gtsam::Vector> responses; std::optional<gtsam::Vector> responses;
Keypoints(const Eigen::MatrixX2d& coordinates) Keypoints(const Eigen::MatrixX2d &_coordinates)
: coordinates(coordinates){}; // std::nullopt : coordinates(_coordinates) {}
}; };
using KeypointsVector = std::vector<Keypoints>; using KeypointsVector = std::vector<Keypoints>;

View File

@ -61,7 +61,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
return noiseModel::Robust::Create( return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel); noiseModel::mEstimator::Huber::Create(1.345), isoModel);
} else { } else {
return isoModel; return std::move(isoModel);
} }
} }

View File

@ -129,7 +129,7 @@ public:
try { try {
return camera.project2(point,H1,H2) - measured_; return camera.project2(point,H1,H2) - measured_;
} }
catch( CheiralityException& e) { catch( CheiralityException& e [[maybe_unused]]) {
if (H1) *H1 = JacobianC::Zero(); if (H1) *H1 = JacobianC::Zero();
if (H2) *H2 = JacobianL::Zero(); if (H2) *H2 = JacobianL::Zero();
//TODO Print the exception via logging //TODO Print the exception via logging
@ -150,7 +150,7 @@ public:
const CAMERA& camera = values.at<CAMERA>(key1); const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2); const LANDMARK& point = values.at<LANDMARK>(key2);
b = measured() - camera.project2(point, H1, H2); b = measured() - camera.project2(point, H1, H2);
} catch (CheiralityException& e) { } catch (CheiralityException& e [[maybe_unused]]) {
H1.setZero(); H1.setZero();
H2.setZero(); H2.setZero();
b.setZero(); b.setZero();

View File

@ -107,9 +107,6 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
"linearizationMode must be set to HESSIAN"); "linearizationMode must be set to HESSIAN");
} }
/** Virtual destructor */
~SmartProjectionRigFactor() override = default;
/** /**
* add a new measurement, corresponding to an observation from pose "poseKey" * add a new measurement, corresponding to an observation from pose "poseKey"
* and taken from the camera in the rig identified by "cameraId" * and taken from the camera in the rig identified by "cameraId"

View File

@ -456,19 +456,14 @@ template <> struct ParseMeasurement<BearingRange2D> {
// The actual parser // The actual parser
std::optional<BinaryMeasurement<BearingRange2D>> std::optional<BinaryMeasurement<BearingRange2D>>
operator()(std::istream &is, const std::string &tag) { operator()(std::istream &is, const std::string &tag) {
if (tag != "BR" && tag != "LANDMARK")
return std::nullopt;
size_t id1, id2; size_t id1, id2;
is >> id1 >> id2; is >> id1 >> id2;
double bearing, range, bearing_std, range_std; double bearing, range, bearing_std, range_std;
if (tag == "BR") { if (tag == "BR") {
is >> bearing >> range >> bearing_std >> range_std; is >> bearing >> range >> bearing_std >> range_std;
} } else if (tag == "LANDMARK") {
// A landmark measurement, converted to bearing-range
// A landmark measurement, converted to bearing-range
if (tag == "LANDMARK") {
double lmx, lmy; double lmx, lmy;
double v1, v2, v3; double v1, v2, v3;
@ -489,6 +484,8 @@ template <> struct ParseMeasurement<BearingRange2D> {
bearing_std = 1; bearing_std = 1;
range_std = 1; range_std = 1;
} }
} else {
return std::nullopt;
} }
// optional filter // optional filter

View File

@ -83,9 +83,6 @@ namespace gtsam {
return *this; return *this;
} }
/// Destructor
virtual ~SymbolicBayesNet() {}
/// @} /// @}
/// @name Testable /// @name Testable

View File

@ -39,7 +39,6 @@ namespace gtsam {
typedef std::shared_ptr<This> shared_ptr; typedef std::shared_ptr<This> shared_ptr;
typedef std::weak_ptr<This> weak_ptr; typedef std::weak_ptr<This> weak_ptr;
SymbolicBayesTreeClique() {} SymbolicBayesTreeClique() {}
virtual ~SymbolicBayesTreeClique() {}
SymbolicBayesTreeClique(const std::shared_ptr<SymbolicConditional>& conditional) : Base(conditional) {} SymbolicBayesTreeClique(const std::shared_ptr<SymbolicConditional>& conditional) : Base(conditional) {}
}; };

View File

@ -79,7 +79,7 @@ namespace gtsam {
/** Create symbolic version of any factor */ /** Create symbolic version of any factor */
explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {} explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {}
virtual ~SymbolicFactor() {} virtual ~SymbolicFactor() override {}
/// Copy this object as its actual derived type. /// Copy this object as its actual derived type.
SymbolicFactor::shared_ptr clone() const { return std::make_shared<This>(*this); } SymbolicFactor::shared_ptr clone() const { return std::make_shared<This>(*this); }

View File

@ -111,9 +111,6 @@ namespace gtsam {
return *this; return *this;
} }
/// Destructor
virtual ~SymbolicFactorGraph() {}
/// @} /// @}
/// @name Testable /// @name Testable

View File

@ -113,9 +113,6 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter
"linearizationMode must be set to HESSIAN"); "linearizationMode must be set to HESSIAN");
} }
/** Virtual destructor */
~SmartProjectionPoseFactorRollingShutter() override = default;
/** /**
* add a new measurement, with 2 pose keys, interpolation factor, and cameraId * add a new measurement, with 2 pose keys, interpolation factor, and cameraId
* @param measured 2-dimensional location of the projection of a single * @param measured 2-dimensional location of the projection of a single

View File

@ -79,9 +79,6 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
const SmartStereoProjectionParams& params = const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()); SmartStereoProjectionParams());
/** Virtual destructor */
~SmartStereoProjectionFactorPP() override = default;
/** /**
* add a new measurement, with a pose key, and an extrinsic pose key * add a new measurement, with a pose key, and an extrinsic pose key
* @param measured is the 3-dimensional location of the projection of a * @param measured is the 3-dimensional location of the projection of a

View File

@ -71,9 +71,6 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const std::optional<Pose3>& body_P_sensor = {}); const std::optional<Pose3>& body_P_sensor = {});
/** Virtual destructor */
~SmartStereoProjectionPoseFactor() override = default;
/** /**
* add a new measurement and pose key * add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a * @param measured is the 2m dimensional location of the projection of a