12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
release/4.3a0
Jose Luis Blanco Claraco 2020-07-26 09:57:54 +02:00
parent 4e3638f6a7
commit 0198c648e3
No known key found for this signature in database
GPG Key ID: D443304FBD70A641
132 changed files with 696 additions and 698 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CppUnitLite/Test.h
View File

@ -64,7 +64,7 @@ protected:
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 () {};\ virtual ~testGroup##testName##Test () {};\
void run (TestResult& result_);} \ 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_)
@ -82,7 +82,7 @@ protected:
class testGroup##testName##Test : public Test \ class testGroup##testName##Test : public Test \
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \
virtual ~testGroup##testName##Test () {};\ virtual ~testGroup##testName##Test () {};\
void run (TestResult& result_);} \ 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_)

2
cmake/GtsamBuildTypes.cmake
View File

@ -112,7 +112,7 @@ else()
$<$<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
-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
-Wsuggest-override -Werror=suggest-override # Enforce the use of the override keyword $<$<COMPILE_LANGUAGE:CXX>:-Wsuggest-override -Werror=suggest-override> # Enforce the use of the override keyword
# #
CACHE STRING "(User editable) Private compiler flags for all configurations.") 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_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.")

4
examples/CameraResectioning.cpp
View File

@ -46,8 +46,8 @@ public:
} }
/// evaluate the error /// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const { boost::none) const override {
PinholeCamera<Cal3_S2> camera(pose, *K_); PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_; return camera.project(P_, H, boost::none, boost::none) - p_;
} }

4
examples/LocalizationExample.cpp
View File

@ -85,7 +85,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
// function, returning a vector of errors when evaluated at the provided variable value. It // 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. // must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const override {
// The measurement function for a GPS-like measurement is simple: // The measurement function for a GPS-like measurement is simple:
// error_x = pose.x - measurement.x // error_x = pose.x - measurement.x
// error_y = pose.y - measurement.y // error_y = pose.y - measurement.y
@ -99,7 +99,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
// The second is a 'clone' function that allows the factor to be copied. Under most // 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 // circumstances, the following code that employs the default copy constructor should
// work fine. // work fine.
virtual gtsam::NonlinearFactor::shared_ptr clone() const { gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }

18
gtsam/base/GenericValue.h
View File

@ -70,7 +70,7 @@ public:
} }
/// equals implementing generic Value interface /// 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 // Cast the base class Value pointer to a templated generic class pointer
const GenericValue& genericValue2 = static_cast<const GenericValue&>(p); const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
// Return the result of using the equals traits for the derived class // Return the result of using the equals traits for the derived class
@ -83,7 +83,7 @@ public:
} }
/// Virtual print function, uses traits /// 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()) << ") "; std::cout << "(" << demangle(typeid(T).name()) << ") ";
traits<T>::Print(value_, str); traits<T>::Print(value_, str);
} }
@ -91,7 +91,7 @@ public:
/** /**
* Create a duplicate object returned as a pointer to the generic Value interface. * 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 GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
return ptr; return ptr;
} }
@ -99,19 +99,19 @@ public:
/** /**
* Destroy and deallocate this object, only if it was originally allocated using clone_(). * Destroy and deallocate this object, only if it was originally allocated using clone_().
*/ */
virtual void deallocate_() const { void deallocate_() const override {
delete this; delete this;
} }
/** /**
* Clone this value (normal clone on the heap, delete with 'delete' operator) * Clone this value (normal clone on the heap, delete with 'delete' operator)
*/ */
virtual boost::shared_ptr<Value> clone() const { boost::shared_ptr<Value> clone() const override {
return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this); return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
} }
/// Generic Value interface version of retract /// 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 // Call retract on the derived class using the retract trait function
const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta); const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
@ -122,7 +122,7 @@ public:
} }
/// Generic Value interface version of localCoordinates /// 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 // Cast the base class Value pointer to a templated generic class pointer
const GenericValue<T>& genericValue2 = const GenericValue<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2); static_cast<const GenericValue<T>&>(value2);
@ -142,12 +142,12 @@ public:
} }
/// Return run-time dimensionality /// Return run-time dimensionality
virtual size_t dim() const { size_t dim() const override {
return traits<T>::GetDimension(value_); return traits<T>::GetDimension(value_);
} }
/// Assignment operator /// 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 // Cast the base class Value pointer to a derived class pointer
const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs); const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);

10
gtsam/base/ThreadsafeException.h
View File

@ -71,12 +71,12 @@ protected:
String(description.begin(), description.end())) { String(description.begin(), description.end())) {
} }
/// Default destructor doesn't have the throw() /// Default destructor doesn't have the noexcept
virtual ~ThreadsafeException() throw () { virtual ~ThreadsafeException() noexcept {
} }
public: public:
virtual const char* what() const throw () { const char* what() const noexcept override {
return description_ ? description_->c_str() : ""; return description_ ? description_->c_str() : "";
} }
}; };
@ -113,8 +113,8 @@ public:
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed> class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{ {
public: public:
CholeskyFailed() throw() {} CholeskyFailed() noexcept {}
virtual ~CholeskyFailed() throw() {} virtual ~CholeskyFailed() noexcept {}
}; };
} // namespace gtsam } // namespace gtsam

4
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
makeNewTasks(makeNewTasks), makeNewTasks(makeNewTasks),
isPostOrderPhase(false) {} isPostOrderPhase(false) {}
tbb::task* execute() tbb::task* execute() override
{ {
if(isPostOrderPhase) if(isPostOrderPhase)
{ {
@ -144,7 +144,7 @@ namespace gtsam {
roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
problemSizeThreshold(problemSizeThreshold) {} problemSizeThreshold(problemSizeThreshold) {}
tbb::task* execute() tbb::task* execute() override
{ {
typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask; typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
// Create data and tasks for our children // Create data and tasks for our children

48
gtsam/discrete/DecisionTree-inl.h
View File

@ -66,42 +66,42 @@ namespace gtsam {
} }
/// Leaf-Leaf equality /// Leaf-Leaf equality
bool sameLeaf(const Leaf& q) const { bool sameLeaf(const Leaf& q) const override {
return constant_ == q.constant_; return constant_ == q.constant_;
} }
/// polymorphic equality: is q is a leaf, could be /// 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)); return (q.isLeaf() && q.sameLeaf(*this));
} }
/** equality up to tolerance */ /** 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<const Leaf*> (&q); const Leaf* other = dynamic_cast<const Leaf*> (&q);
if (!other) return false; if (!other) return false;
return std::abs(double(this->constant_ - other->constant_)) < tol; return std::abs(double(this->constant_ - other->constant_)) < tol;
} }
/** print */ /** print */
void print(const std::string& s) const { void print(const std::string& s) const override {
bool showZero = true; bool showZero = true;
if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl;
} }
/** to graphviz file */ /** 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=\"" if (showZero || constant_) os << "\"" << this->id() << "\" [label=\""
<< boost::format("%4.2g") % constant_ << boost::format("%4.2g") % constant_
<< "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55,
} }
/** evaluate */ /** evaluate */
const Y& operator()(const Assignment<L>& x) const { const Y& operator()(const Assignment<L>& x) const override {
return constant_; return constant_;
} }
/** apply unary operator */ /** apply unary operator */
NodePtr apply(const Unary& op) const { NodePtr apply(const Unary& op) const override {
NodePtr f(new Leaf(op(constant_))); NodePtr f(new Leaf(op(constant_)));
return f; return f;
} }
@ -111,27 +111,27 @@ namespace gtsam {
// Simply calls apply on argument to call correct virtual method: // 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(gL) -> gL.apply_g_op_fL(fL) (below)
// fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice) // 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); return g.apply_g_op_fL(*this, op);
} }
// Applying binary operator to two leaves results in a leaf // 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 NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL
return h; return h;
} }
// If second argument is a Choice node, call it's apply with leaf as second // 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 return fC.apply_fC_op_gL(*this, op); // operand order back to normal
} }
/** choose a branch, create new memory ! */ /** 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())); return NodePtr(new Leaf(constant()));
} }
bool isLeaf() const { return true; } bool isLeaf() const override { return true; }
}; // Leaf }; // Leaf
@ -175,7 +175,7 @@ namespace gtsam {
return f; return f;
} }
bool isLeaf() const { return false; } bool isLeaf() const override { return false; }
/** Constructor, given choice label and mandatory expected branch count */ /** Constructor, given choice label and mandatory expected branch count */
Choice(const L& label, size_t count) : Choice(const L& label, size_t count) :
@ -236,7 +236,7 @@ namespace gtsam {
} }
/** print (as a tree) */ /** 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 << s << " Choice(";
// std::cout << this << ","; // std::cout << this << ",";
std::cout << label_ << ") " << std::endl; std::cout << label_ << ") " << std::endl;
@ -245,7 +245,7 @@ namespace gtsam {
} }
/** output to graphviz (as a a graph) */ /** 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_ os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_
<< "\"]\n"; << "\"]\n";
for (size_t i = 0; i < branches_.size(); i++) { for (size_t i = 0; i < branches_.size(); i++) {
@ -266,17 +266,17 @@ namespace gtsam {
} }
/// Choice-Leaf equality: always false /// Choice-Leaf equality: always false
bool sameLeaf(const Leaf& q) const { bool sameLeaf(const Leaf& q) const override {
return false; return false;
} }
/// polymorphic equality: if q is a leaf, could be... /// 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)); return (q.isLeaf() && q.sameLeaf(*this));
} }
/** equality up to tolerance */ /** 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<const Choice*> (&q); const Choice* other = dynamic_cast<const Choice*> (&q);
if (!other) return false; if (!other) return false;
if (this->label_ != other->label_) return false; if (this->label_ != other->label_) return false;
@ -288,7 +288,7 @@ namespace gtsam {
} }
/** evaluate */ /** evaluate */
const Y& operator()(const Assignment<L>& x) const { const Y& operator()(const Assignment<L>& x) const override {
#ifndef NDEBUG #ifndef NDEBUG
typename Assignment<L>::const_iterator it = x.find(label_); typename Assignment<L>::const_iterator it = x.find(label_);
if (it == x.end()) { if (it == x.end()) {
@ -314,7 +314,7 @@ namespace gtsam {
} }
/** apply unary operator */ /** apply unary operator */
NodePtr apply(const Unary& op) const { NodePtr apply(const Unary& op) const override {
boost::shared_ptr<Choice> r(new Choice(label_, *this, op)); boost::shared_ptr<Choice> r(new Choice(label_, *this, op));
return Unique(r); return Unique(r);
} }
@ -324,12 +324,12 @@ namespace gtsam {
// Simply calls apply on argument to call correct virtual method: // 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(gL) -> gL.apply_g_op_fC(fC) -> (Leaf)
// fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below) // 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); return g.apply_g_op_fC(*this, op);
} }
// If second argument of binary op is Leaf node, recurse on branches // 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<Choice> h(new Choice(label(), nrChoices())); boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
for(NodePtr branch: branches_) for(NodePtr branch: branches_)
h->push_back(fL.apply_f_op_g(*branch, op)); 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 // 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<Choice> h(new Choice(fC, *this, op)); boost::shared_ptr<Choice> h(new Choice(fC, *this, op));
return Unique(h); return Unique(h);
} }
@ -352,7 +352,7 @@ namespace gtsam {
} }
/** choose a branch, recursively */ /** 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) if (label_ == label)
return branches_[index]; // choose branch return branches_[index]; // choose branch

12
gtsam/discrete/DecisionTreeFactor.h
View File

@ -69,23 +69,23 @@ namespace gtsam {
/// @{ /// @{
/// equality /// equality
bool equals(const DiscreteFactor& other, double tol = 1e-9) const; bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
// print // print
virtual void print(const std::string& s = "DecisionTreeFactor:\n", void print(const std::string& s = "DecisionTreeFactor:\n",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/// Value is just look up in AlgebraicDecisonTree /// 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); return Potentials::operator()(values);
} }
/// multiply two factors /// multiply two factors
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
return apply(f, ADT::Ring::mul); return apply(f, ADT::Ring::mul);
} }
@ -95,7 +95,7 @@ namespace gtsam {
} }
/// Convert into a decisiontree /// Convert into a decisiontree
virtual DecisionTreeFactor toDecisionTreeFactor() const { DecisionTreeFactor toDecisionTreeFactor() const override {
return *this; return *this;
} }

6
gtsam/discrete/DiscreteConditional.h
View File

@ -85,10 +85,10 @@ public:
/// GTSAM-style print /// GTSAM-style print
void print(const std::string& s = "Discrete Conditional: ", void print(const std::string& s = "Discrete Conditional: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// GTSAM-style equals /// 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 /// @name Standard Interface
@ -102,7 +102,7 @@ public:
} }
/// Evaluate, just look up in AlgebraicDecisonTree /// Evaluate, just look up in AlgebraicDecisonTree
virtual double operator()(const Values& values) const { double operator()(const Values& values) const override {
return Potentials::operator()(values); return Potentials::operator()(values);
} }

4
gtsam/geometry/Cal3DS2.h
View File

@ -60,7 +60,7 @@ public:
/// @{ /// @{
/// print with optional string /// 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 /// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const; bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@ -86,7 +86,7 @@ public:
/// @{ /// @{
/// @return a deep copy of this object /// @return a deep copy of this object
virtual boost::shared_ptr<Base> clone() const { boost::shared_ptr<Base> clone() const override {
return boost::shared_ptr<Base>(new Cal3DS2(*this)); return boost::shared_ptr<Base>(new Cal3DS2(*this));
} }

2
gtsam/geometry/Cal3DS2_Base.h
View File

@ -69,7 +69,7 @@ public:
/// @{ /// @{
/// print with optional string /// 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 /// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;

2
gtsam/geometry/Cal3Unified.h
View File

@ -75,7 +75,7 @@ public:
/// @{ /// @{
/// print with optional string /// 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 /// assert equality up to a tolerance
bool equals(const Cal3Unified& K, double tol = 10e-9) const; bool equals(const Cal3Unified& K, double tol = 10e-9) const;

2
gtsam/geometry/PinholeCamera.h
View File

@ -175,7 +175,7 @@ public:
} }
/// return calibration /// return calibration
const Calibration& calibration() const { const Calibration& calibration() const override {
return K_; return K_;
} }

2
gtsam/geometry/PinholePose.h
View File

@ -361,7 +361,7 @@ public:
} }
/// return calibration /// return calibration
virtual const CALIBRATION& calibration() const { const CALIBRATION& calibration() const override {
return *K_; return *K_;
} }

2
gtsam/geometry/PinholeSet.h
View File

@ -45,7 +45,7 @@ public:
/// @{ /// @{
/// print /// print
virtual void print(const std::string& s = "") const { void print(const std::string& s = "") const override {
Base::print(s); Base::print(s);
} }

2
gtsam/inference/BayesTree.h
View File

@ -299,7 +299,7 @@ namespace gtsam {
this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); 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); clique->print(s + "stored clique", formatter);
} }
}; };

2
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -100,7 +100,7 @@ namespace gtsam {
bool equals(const DERIVED& other, double tol = 1e-9) const; bool equals(const DERIVED& other, double tol = 1e-9) const;
/** print this node */ /** 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 /// @name Standard Interface

6
gtsam/inference/inferenceExceptions.h
View File

@ -28,9 +28,9 @@ namespace gtsam {
* with an ordering that does not include all of the variables. */ * with an ordering that does not include all of the variables. */
class InconsistentEliminationRequested : public std::exception { class InconsistentEliminationRequested : public std::exception {
public: public:
InconsistentEliminationRequested() throw() {} InconsistentEliminationRequested() noexcept {}
virtual ~InconsistentEliminationRequested() throw() {} virtual ~InconsistentEliminationRequested() noexcept {}
virtual const char* what() const throw() { const char* what() const noexcept override {
return return
"An inference algorithm was called with inconsistent arguments. The\n" "An inference algorithm was called with inconsistent arguments. The\n"
"factor graph, ordering, or variable index were inconsistent with each\n" "factor graph, ordering, or variable index were inconsistent with each\n"

2
gtsam/linear/BinaryJacobianFactor.h
View File

@ -49,7 +49,7 @@ struct BinaryJacobianFactor: JacobianFactor {
// Fixed-size matrix update // Fixed-size matrix update
void updateHessian(const KeyVector& infoKeys, void updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const { SymmetricBlockMatrix* info) const override {
gttic(updateHessian_BinaryJacobianFactor); gttic(updateHessian_BinaryJacobianFactor);
// Whiten the factor if it has a noise model // Whiten the factor if it has a noise model
const SharedDiagonal& model = get_model(); const SharedDiagonal& model = get_model();

2
gtsam/linear/ConjugateGradientSolver.h
View File

@ -80,7 +80,7 @@ public:
void print() const { Base::print(); } 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 std::string blasTranslator(const BLASKernel k) ;
static BLASKernel blasTranslator(const std::string &s) ; static BLASKernel blasTranslator(const std::string &s) ;

4
gtsam/linear/GaussianConditional.h
View File

@ -88,10 +88,10 @@ namespace gtsam {
/** print */ /** print */
void print(const std::string& = "GaussianConditional", void print(const std::string& = "GaussianConditional",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/** equals function */ /** 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 */ /** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); } constABlock R() const { return Ab_.range(0, nrFrontals()); }

2
gtsam/linear/GaussianDensity.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
/// print /// print
void print(const std::string& = "GaussianDensity", void print(const std::string& = "GaussianDensity",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// Mean \f$ \mu = R^{-1} d \f$ /// Mean \f$ \mu = R^{-1} d \f$
Vector mean() const; Vector mean() const;

8
gtsam/linear/LossFunctions.h
View File

@ -134,10 +134,10 @@ class GTSAM_EXPORT Null : public Base {
Null(const ReweightScheme reweight = Block) : Base(reweight) {} Null(const ReweightScheme reweight = Block) : Base(reweight) {}
~Null() {} ~Null() {}
double weight(double /*error*/) const { return 1.0; } double weight(double /*error*/) const override { return 1.0; }
double loss(double distance) const { return 0.5 * distance * distance; } double loss(double distance) const override { return 0.5 * distance * distance; }
void print(const std::string &s) const; void print(const std::string &s) const override;
bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
static shared_ptr Create(); static shared_ptr Create();
private: private:

6
gtsam/linear/PCGSolver.h
View File

@ -41,7 +41,7 @@ public:
PCGSolverParameters() { PCGSolverParameters() {
} }
virtual void print(std::ostream &os) const; void print(std::ostream &os) const override;
/* interface to preconditioner parameters */ /* interface to preconditioner parameters */
inline const PreconditionerParameters& preconditioner() const { inline const PreconditionerParameters& preconditioner() const {
@ -77,9 +77,9 @@ public:
using IterativeSolver::optimize; using IterativeSolver::optimize;
virtual VectorValues optimize(const GaussianFactorGraph &gfg, VectorValues optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda, const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial); const VectorValues &initial) override;
}; };

16
gtsam/linear/Preconditioner.h
View File

@ -111,13 +111,13 @@ public:
virtual ~DummyPreconditioner() {} virtual ~DummyPreconditioner() {}
/* Computation Interfaces for raw vector */ /* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const { x = y; } void solve(const Vector& y, Vector &x) const override { x = y; }
virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } void transposeSolve(const Vector& y, Vector& x) const override { x = y; }
virtual void build( void build(
const GaussianFactorGraph &gfg, const GaussianFactorGraph &gfg,
const KeyInfo &info, const KeyInfo &info,
const std::map<Key,Vector> &lambda const std::map<Key,Vector> &lambda
) {} ) override {}
}; };
/*******************************************************************************************/ /*******************************************************************************************/
@ -135,13 +135,13 @@ public:
virtual ~BlockJacobiPreconditioner() ; virtual ~BlockJacobiPreconditioner() ;
/* Computation Interfaces for raw vector */ /* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const; void solve(const Vector& y, Vector &x) const override;
virtual void transposeSolve(const Vector& y, Vector& x) const ; void transposeSolve(const Vector& y, Vector& x) const override;
virtual void build( void build(
const GaussianFactorGraph &gfg, const GaussianFactorGraph &gfg,
const KeyInfo &info, const KeyInfo &info,
const std::map<Key,Vector> &lambda const std::map<Key,Vector> &lambda
) ; ) override;
protected: protected:

8
gtsam/linear/RegularHessianFactor.h
View File

@ -109,8 +109,8 @@ private:
public: public:
/** y += alpha * A'*A*x */ /** y += alpha * A'*A*x */
virtual void multiplyHessianAdd(double alpha, const VectorValues& x, void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const { VectorValues& y) const override {
HessianFactor::multiplyHessianAdd(alpha, x, y); HessianFactor::multiplyHessianAdd(alpha, x, y);
} }
@ -182,7 +182,7 @@ public:
} }
/** Return the diagonal of the Hessian for this factor (raw memory version) */ /** 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 // Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { 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 ?? /// 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 // Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {

10
gtsam/linear/RegularJacobianFactor.h
View File

@ -70,8 +70,8 @@ public:
using JacobianFactor::multiplyHessianAdd; using JacobianFactor::multiplyHessianAdd;
/** y += alpha * A'*A*x */ /** y += alpha * A'*A*x */
virtual void multiplyHessianAdd(double alpha, const VectorValues& x, void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const { VectorValues& y) const override {
JacobianFactor::multiplyHessianAdd(alpha, x, y); JacobianFactor::multiplyHessianAdd(alpha, x, y);
} }
@ -106,7 +106,7 @@ public:
using GaussianFactor::hessianDiagonal; using GaussianFactor::hessianDiagonal;
/// Raw memory access version of 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 // Loop over all variables in the factor
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal // Get the diagonal block, and insert its diagonal
@ -125,12 +125,12 @@ public:
} }
/// Expose base class gradientAtZero /// Expose base class gradientAtZero
virtual VectorValues gradientAtZero() const { VectorValues gradientAtZero() const override {
return JacobianFactor::gradientAtZero(); return JacobianFactor::gradientAtZero();
} }
/// Raw memory access version of gradientAtZero /// Raw memory access version of gradientAtZero
void gradientAtZero(double* d) const { void gradientAtZero(double* d) const override {
// Get vector b not weighted // Get vector b not weighted
Vector b = getb(); Vector b = getb();

2
gtsam/linear/SubgraphSolver.h
View File

@ -38,7 +38,7 @@ struct GTSAM_EXPORT SubgraphSolverParameters
explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: builderParams(p) {} : builderParams(p) {}
void print() const { Base::print(); } void print() const { Base::print(); }
virtual void print(std::ostream &os) const { void print(std::ostream &os) const override {
Base::print(os); Base::print(os);
} }
}; };

6
gtsam/linear/linearExceptions.cpp
View File

@ -24,7 +24,7 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
const char* IndeterminantLinearSystemException::what() const throw() const char* IndeterminantLinearSystemException::what() const noexcept
{ {
if(!description_) { if(!description_) {
description_ = String( description_ = String(
@ -43,7 +43,7 @@ more information.");
} }
/* ************************************************************************* */ /* ************************************************************************* */
const char* InvalidNoiseModel::what() const throw() { const char* InvalidNoiseModel::what() const noexcept {
if(description_.empty()) if(description_.empty())
description_ = (boost::format( description_ = (boost::format(
"A JacobianFactor was attempted to be constructed or modified to use a\n" "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()) if(description_.empty())
description_ = (boost::format( description_ = (boost::format(
"A JacobianFactor was attempted to be constructed with a matrix block of\n" "A JacobianFactor was attempted to be constructed with a matrix block of\n"

14
gtsam/linear/linearExceptions.h
View File

@ -94,10 +94,10 @@ namespace gtsam {
class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> { class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
Key j_; Key j_;
public: public:
IndeterminantLinearSystemException(Key j) throw() : j_(j) {} IndeterminantLinearSystemException(Key j) noexcept : j_(j) {}
virtual ~IndeterminantLinearSystemException() throw() {} virtual ~IndeterminantLinearSystemException() noexcept {}
Key nearbyVariable() const { return j_; } 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) : InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) :
factorDims(factorDims), noiseModelDims(noiseModelDims) {} factorDims(factorDims), noiseModelDims(noiseModelDims) {}
virtual ~InvalidNoiseModel() throw() {} virtual ~InvalidNoiseModel() noexcept {}
virtual const char* what() const throw(); const char* what() const noexcept override;
private: private:
mutable std::string description_; mutable std::string description_;
@ -128,9 +128,9 @@ namespace gtsam {
InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) : InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) :
factorRows(factorRows), blockRows(blockRows) {} factorRows(factorRows), blockRows(blockRows) {}
virtual ~InvalidMatrixBlock() throw() {} virtual ~InvalidMatrixBlock() noexcept {}
virtual const char* what() const throw(); const char* what() const noexcept override;
private: private:
mutable std::string description_; mutable std::string description_;

10
gtsam/navigation/AHRSFactor.h
View File

@ -158,14 +158,14 @@ public:
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const; gtsam::NonlinearFactor::shared_ptr clone() const override;
/// print /// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
/// equals /// 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. /// Access the preintegrated measurements.
const PreintegratedAhrsMeasurements& preintegratedMeasurements() const { const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
@ -178,7 +178,7 @@ public:
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const Vector3& bias, boost::optional<Matrix&> H1 = boost::none, const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const; boost::none) const override;
/// predicted states from IMU /// predicted states from IMU
/// TODO(frank): relationship with PIM predict ?? /// TODO(frank): relationship with PIM predict ??

24
gtsam/navigation/AttitudeFactor.h
View File

@ -108,21 +108,21 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
/** equals */ /** 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 of errors */
virtual Vector evaluateError(const Rot3& nRb, // Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const override {
return attitudeError(nRb, H); return attitudeError(nRb, H);
} }
@ -182,21 +182,21 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
/** equals */ /** 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 of errors */
virtual Vector evaluateError(const Pose3& nTb, // Vector evaluateError(const Pose3& nTb, //
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const override {
Vector e = attitudeError(nTb.rotation(), H); Vector e = attitudeError(nTb.rotation(), H);
if (H) { if (H) {
Matrix H23 = *H; Matrix H23 = *H;

14
gtsam/navigation/CombinedImuFactor.h
View File

@ -87,8 +87,8 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g))); return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
} }
void print(const std::string& s="") const; void print(const std::string& s="") const override;
bool equals(const PreintegratedRotationParams& other, double tol) const; bool equals(const PreintegratedRotationParams& other, double tol) const override;
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
@ -305,7 +305,7 @@ public:
virtual ~CombinedImuFactor() {} virtual ~CombinedImuFactor() {}
/// @return a deep copy of this factor /// @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 */ /** implement functions needed for Testable */
@ -314,11 +314,11 @@ public:
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const CombinedImuFactor&); const CombinedImuFactor&);
/// print /// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
/// equals /// 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. */ /** Access the preintegrated measurements. */
@ -336,7 +336,7 @@ public:
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none, boost::optional<Matrix&> H3 = boost::none, boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 = boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const; boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated typename /// @deprecated typename

20
gtsam/navigation/GPSFactor.h
View File

@ -65,21 +65,21 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/// print /// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
/// equals /// 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 of errors
Vector evaluateError(const Pose3& p, Vector evaluateError(const Pose3& p,
boost::optional<Matrix&> H = boost::none) const; boost::optional<Matrix&> H = boost::none) const override;
inline const Point3 & measurementIn() const { inline const Point3 & measurementIn() const {
return nT_; return nT_;
@ -137,21 +137,21 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/// print /// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
/// equals /// 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 of errors
Vector evaluateError(const NavState& p, Vector evaluateError(const NavState& p,
boost::optional<Matrix&> H = boost::none) const; boost::optional<Matrix&> H = boost::none) const override;
inline const Point3 & measurementIn() const { inline const Point3 & measurementIn() const {
return nT_; return nT_;

20
gtsam/navigation/ImuFactor.h
View File

@ -217,14 +217,14 @@ public:
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const; gtsam::NonlinearFactor::shared_ptr clone() const override;
/// @name Testable /// @name Testable
/// @{ /// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&); GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
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. */ /** Access the preintegrated measurements. */
@ -241,7 +241,7 @@ public:
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 = const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none, boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 = boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const; boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
#ifdef GTSAM_TANGENT_PREINTEGRATION #ifdef GTSAM_TANGENT_PREINTEGRATION
/// Merge two pre-integrated measurement classes /// Merge two pre-integrated measurement classes
@ -315,14 +315,14 @@ public:
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const; gtsam::NonlinearFactor::shared_ptr clone() const override;
/// @name Testable /// @name Testable
/// @{ /// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&); GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
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. */ /** Access the preintegrated measurements. */
@ -338,7 +338,7 @@ public:
const imuBias::ConstantBias& bias_i, // const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const; boost::optional<Matrix&> H3 = boost::none) const override;
private: private:

8
gtsam/navigation/MagFactor.h
View File

@ -53,7 +53,7 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor(*this))); NonlinearFactor::shared_ptr(new MagFactor(*this)));
} }
@ -102,7 +102,7 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor1(*this))); NonlinearFactor::shared_ptr(new MagFactor1(*this)));
} }
@ -138,7 +138,7 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor2(*this))); NonlinearFactor::shared_ptr(new MagFactor2(*this)));
} }
@ -179,7 +179,7 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor3(*this))); NonlinearFactor::shared_ptr(new MagFactor3(*this)));
} }

4
gtsam/navigation/PreintegrationParams.h
View File

@ -56,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g))); return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
} }
void print(const std::string& s="") const; void print(const std::string& s="") const override;
bool equals(const PreintegratedRotationParams& other, double tol) const; bool equals(const PreintegratedRotationParams& other, double tol) const override;
void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }

14
gtsam/nonlinear/ExpressionFactor.h
View File

@ -68,13 +68,13 @@ protected:
/// print relies on Testable traits being defined for T /// print relies on Testable traits being defined for T
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
NoiseModelFactor::print(s, keyFormatter); NoiseModelFactor::print(s, keyFormatter);
traits<T>::Print(measured_, "ExpressionFactor with measurement: "); traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
} }
/// equals relies on Testable traits being defined for T /// 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<const ExpressionFactor*>(&f); const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
return p && NoiseModelFactor::equals(f, tol) && return p && NoiseModelFactor::equals(f, tol) &&
traits<T>::Equals(measured_, p->measured_, tol) && traits<T>::Equals(measured_, p->measured_, tol) &&
@ -86,8 +86,8 @@ protected:
* We override this method to provide * We override this method to provide
* both the function evaluation and its derivative(s) in H. * both the function evaluation and its derivative(s) in H.
*/ */
virtual Vector unwhitenedError(const Values& x, Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const { boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if (H) { if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
// NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
@ -99,7 +99,7 @@ protected:
} }
} }
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!active(x)) if (!active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
@ -138,7 +138,7 @@ protected:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -263,7 +263,7 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
private: private:
/// Return an expression that predicts the measurement given Values /// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const { Expression<T> expression() const override {
return expression(this->keys_[0], this->keys_[1]); return expression(this->keys_[0], this->keys_[1]);
} }

8
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -82,13 +82,13 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
virtual ~FunctorizedFactor() {} virtual ~FunctorizedFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this))); NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
} }
Vector evaluateError(const T &params, Vector evaluateError(const T &params,
boost::optional<Matrix &> H = boost::none) const { boost::optional<Matrix &> H = boost::none) const override {
R x = func_(params, H); R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x); Vector error = traits<R>::Local(measured_, x);
return error; return error;
@ -97,7 +97,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
/// @name Testable /// @name Testable
/// @{ /// @{
void print(const std::string &s = "", void print(const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key()) << ")" << std::endl; << keyFormatter(this->key()) << ")" << std::endl;
@ -106,7 +106,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
<< std::endl; << 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<R, T> *e = const FunctorizedFactor<R, T> *e =
dynamic_cast<const FunctorizedFactor<R, T> *>(&other); dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
return e && Base::equals(other, tol) && return e && Base::equals(other, tol) &&

2
gtsam/nonlinear/ISAM2Clique.h
View File

@ -85,7 +85,7 @@ class GTSAM_EXPORT ISAM2Clique
/** print this node */ /** print this node */
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
void optimizeWildfire(const KeySet& replaced, double threshold, void optimizeWildfire(const KeySet& replaced, double threshold,
KeySet* changed, VectorValues* delta, KeySet* changed, VectorValues* delta,

12
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -59,10 +59,10 @@ public:
// Testable // Testable
/** print */ /** 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 */ /** 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 // NonlinearFactor
@ -74,10 +74,10 @@ public:
* *
* @return nonlinear error if linearizationPoint present, zero otherwise * @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 */ /** 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 */ /** Extract the linearization point used in recalculating error */
const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; } const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; }
@ -98,7 +98,7 @@ public:
* TODO: better approximation of relinearization * TODO: better approximation of relinearization
* TODO: switchable modes for approximation technique * 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 * Creates an anti-factor directly
@ -116,7 +116,7 @@ public:
* *
* Clones the underlying linear factor * Clones the underlying linear factor
*/ */
NonlinearFactor::shared_ptr clone() const { NonlinearFactor::shared_ptr clone() const override {
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_)); return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
} }

26
gtsam/nonlinear/NonlinearEquality.h
View File

@ -107,15 +107,15 @@ public:
/// @name Testable /// @name Testable
/// @{ /// @{
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
traits<VALUE>::Print(feasible_, "Feasible Point:\n"); traits<VALUE>::Print(feasible_, "Feasible Point:\n");
std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_) << std::endl; std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_) << std::endl;
} }
/** Check if two factors are equal */ /** 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<const This*>(&f); const This* e = dynamic_cast<const This*>(&f);
return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol) return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
&& std::abs(error_gain_ - e->error_gain_) < tol; && std::abs(error_gain_ - e->error_gain_) < tol;
@ -126,7 +126,7 @@ public:
/// @{ /// @{
/** actual error function calculation */ /** actual error function calculation */
virtual double error(const Values& c) const { double error(const Values& c) const override {
const T& xj = c.at<T>(this->key()); const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c); Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) { if (allow_error_ || !compare_(xj, feasible_)) {
@ -138,7 +138,7 @@ public:
/** error function */ /** error function */
Vector evaluateError(const T& xj, Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const override {
const size_t nj = traits<T>::GetDimension(feasible_); const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) { if (allow_error_) {
if (H) if (H)
@ -158,7 +158,7 @@ public:
} }
// Linearize is over-written, because base linearization tries to whiten // 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<T>(this->key()); const T& xj = x.at<T>(this->key());
Matrix A; Matrix A;
Vector b = evaluateError(xj, A); Vector b = evaluateError(xj, A);
@ -168,7 +168,7 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -242,14 +242,14 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/** g(x) with optional derivative */ /** g(x) with optional derivative */
Vector evaluateError(const X& x1, Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const override {
if (H) if (H)
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1)); (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
@ -257,8 +257,8 @@ public:
} }
/** Print */ /** Print */
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key()) std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
<< ")," << "\n"; << ")," << "\n";
this->noiseModel_->print(); this->noiseModel_->print();
@ -317,14 +317,14 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/** g(x) with optional derivative2 */ /** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 = Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
static const size_t p = traits<X>::dimension; static const size_t p = traits<X>::dimension;
if (H1) *H1 = -Matrix::Identity(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p); if (H2) *H2 = Matrix::Identity(p,p);

26
gtsam/nonlinear/NonlinearFactor.h
View File

@ -31,7 +31,7 @@
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
#define ADD_CLONE_NONLINEAR_FACTOR(Derived) \ #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>( \ return boost::static_pointer_cast<gtsam::NonlinearFactor>( \
gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); } gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }
#endif #endif
@ -195,14 +195,14 @@ protected:
public: public:
/** Print */ /** Print */
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** Check if two factors are equal */ /** 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) */ /** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const { size_t dim() const override {
return noiseModel_->dim(); 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 * 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. * 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, * Linearize a non-linearFactorN to get a GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * \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$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const; boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
@ -315,7 +315,7 @@ public:
/** Calls the 1-key specific version of evaluateError, which is pure virtual /** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. * so must be implemented in the derived class.
*/ */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) { if(this->active(x)) {
const X& x1 = x.at<X>(keys_[0]); const X& x1 = x.at<X>(keys_[0]);
if(H) { if(H) {
@ -389,7 +389,7 @@ public:
/** Calls the 2-key specific version of evaluateError, which is pure virtual /** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) { if(this->active(x)) {
const X1& x1 = x.at<X1>(keys_[0]); const X1& x1 = x.at<X1>(keys_[0]);
const X2& x2 = x.at<X2>(keys_[1]); const X2& x2 = x.at<X2>(keys_[1]);
@ -467,7 +467,7 @@ public:
/** Calls the 3-key specific version of evaluateError, which is pure virtual /** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(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 /** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(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 /** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(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 /** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);

10
gtsam/nonlinear/PriorFactor.h
View File

@ -65,15 +65,15 @@ namespace gtsam {
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s,
DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
traits<T>::Print(prior_, " prior mean: "); traits<T>::Print(prior_, " prior mean: ");
if (this->noiseModel_) if (this->noiseModel_)
@ -83,7 +83,7 @@ namespace gtsam {
} }
/** equals */ /** 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<const This*> (&expected); const This* e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol); return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
} }
@ -91,7 +91,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x)); if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z) // manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians. // TODO(ASL) Add Jacobians.

8
gtsam/nonlinear/Values.cpp
View File

@ -214,7 +214,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
const char* ValuesKeyAlreadyExists::what() const throw() { const char* ValuesKeyAlreadyExists::what() const noexcept {
if(message_.empty()) if(message_.empty())
message_ = message_ =
"Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists."; "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()) if(message_.empty())
message_ = message_ =
"Attempting to " + std::string(operation_) + " the key \"" + "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()) { if(message_.empty()) {
std::string storedTypeName = demangle(storedTypeId_.name()); std::string storedTypeName = demangle(storedTypeId_.name());
std::string requestedTypeName = demangle(requestedTypeId_.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()) { if(message_.empty()) {
ostringstream oss; ostringstream oss;
oss oss

36
gtsam/nonlinear/Values.h
View File

@ -432,16 +432,16 @@ namespace gtsam {
public: public:
/// Construct with the key-value pair attempted to be added /// Construct with the key-value pair attempted to be added
ValuesKeyAlreadyExists(Key key) throw() : ValuesKeyAlreadyExists(Key key) noexcept :
key_(key) {} key_(key) {}
virtual ~ValuesKeyAlreadyExists() throw() {} virtual ~ValuesKeyAlreadyExists() noexcept {}
/// The duplicate key that was attempted to be added /// 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 /// 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: public:
/// Construct with the key that does not exist in the values /// 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) {} operation_(operation), key_(key) {}
virtual ~ValuesKeyDoesNotExist() throw() {} virtual ~ValuesKeyDoesNotExist() noexcept {}
/// The key that was attempted to be accessed that does not exist /// 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 /// 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: public:
/// Construct with the key that does not exist in the values /// Construct with the key that does not exist in the values
ValuesIncorrectType(Key key, 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) {} key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
virtual ~ValuesIncorrectType() throw() {} virtual ~ValuesIncorrectType() noexcept {}
/// The key that was attempted to be accessed that does not exist /// 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 /// The typeid of the value stores in the Values
const std::type_info& storedTypeId() const { return storedTypeId_; } const std::type_info& storedTypeId() const { return storedTypeId_; }
@ -495,18 +495,18 @@ namespace gtsam {
const std::type_info& requestedTypeId() const { return requestedTypeId_; } const std::type_info& requestedTypeId() const { return requestedTypeId_; }
/// The message to be displayed to the user /// 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 { class DynamicValuesMismatched : public std::exception {
public: 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"; 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_; mutable std::string message_;
public: 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) { 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;
}; };
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -109,7 +109,7 @@ namespace gtsam {
/// Print /// Print
void print(const std::string& p = "WhiteNoiseFactor", void print(const std::string& p = "WhiteNoiseFactor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(p, keyFormatter); Base::print(p, keyFormatter);
std::cout << p + ".z: " << z_ << std::endl; std::cout << p + ".z: " << z_ << std::endl;
} }
@ -119,12 +119,12 @@ namespace gtsam {
/// @{ /// @{
/// get the dimension of the factor (number of rows on linearization) /// get the dimension of the factor (number of rows on linearization)
virtual size_t dim() const { size_t dim() const override {
return 2; return 2;
} }
/// Calculate the error of the factor, typically equal to log-likelihood /// 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<double>(meanKey_), x.at<double>(precisionKey_)); return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
} }
@ -153,7 +153,7 @@ namespace gtsam {
/// @{ /// @{
/// linearize returns a Hessianfactor that is an approximation of error(p) /// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
double u = x.at<double>(meanKey_); double u = x.at<double>(meanKey_);
double p = x.at<double>(precisionKey_); double p = x.at<double>(precisionKey_);
Key j1 = meanKey_; Key j1 = meanKey_;
@ -163,7 +163,7 @@ namespace gtsam {
// TODO: Frank commented this out for now, can it go? // TODO: Frank commented this out for now, can it go?
// /// @return a deep copy of this factor // /// @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>( // return boost::static_pointer_cast<gtsam::NonlinearFactor>(
// gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // gtsam::NonlinearFactor::shared_ptr(new This(*this))); }

30
gtsam/nonlinear/internal/CallRecord.h
View File

@ -144,43 +144,43 @@ private:
return static_cast<const Derived&>(*this); return static_cast<const Derived&>(*this);
} }
virtual void _print(const std::string& indent) const { void _print(const std::string& indent) const override {
derived().print(indent); derived().print(indent);
} }
// Called from base class non-virtual inline method startReverseAD2 // Called from base class non-virtual inline method startReverseAD2
// Calls non-virtual function startReverseAD4, implemented in Derived (ExpressionNode::Record) // 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); 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); derived().reverseAD4(dFdT, jacobians);
} }
virtual void _reverseAD3( void _reverseAD3(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT, const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians); derived().reverseAD4(dFdT, jacobians);
} }
virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT, void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians); derived().reverseAD4(dFdT, jacobians);
} }
virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT, void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians); derived().reverseAD4(dFdT, jacobians);
} }
virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT, void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians); derived().reverseAD4(dFdT, jacobians);
} }
virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT, void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians); derived().reverseAD4(dFdT, jacobians);
} }
virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT, void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians); derived().reverseAD4(dFdT, jacobians);
} }
}; };

80
gtsam/nonlinear/internal/ExpressionNode.h
View File

@ -135,18 +135,18 @@ public:
} }
/// Print /// Print
virtual void print(const std::string& indent = "") const { void print(const std::string& indent = "") const override {
std::cout << indent << "Constant" << std::endl; std::cout << indent << "Constant" << std::endl;
} }
/// Return value /// Return value
virtual T value(const Values& values) const { T value(const Values& values) const override {
return constant_; return constant_;
} }
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const { ExecutionTraceStorage* traceStorage) const override {
return constant_; return constant_;
} }
@ -176,30 +176,30 @@ public:
} }
/// Print /// 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; std::cout << indent << "Leaf, key = " << DefaultKeyFormatter(key_) << std::endl;
} }
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { std::set<Key> keys() const override {
std::set<Key> keys; std::set<Key> keys;
keys.insert(key_); keys.insert(key_);
return keys; return keys;
} }
/// Return dimensions for each argument /// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const { void dims(std::map<Key, int>& map) const override {
map[key_] = traits<T>::dimension; map[key_] = traits<T>::dimension;
} }
/// Return value /// Return value
virtual T value(const Values& values) const { T value(const Values& values) const override {
return values.at<T>(key_); return values.at<T>(key_);
} }
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const { ExecutionTraceStorage* traceStorage) const override {
trace.setLeaf(key_); trace.setLeaf(key_);
return values.at<T>(key_); return values.at<T>(key_);
} }
@ -248,23 +248,23 @@ public:
} }
/// Print /// Print
virtual void print(const std::string& indent = "") const { void print(const std::string& indent = "") const override {
std::cout << indent << "UnaryExpression" << std::endl; std::cout << indent << "UnaryExpression" << std::endl;
expression1_->print(indent + " "); expression1_->print(indent + " ");
} }
/// Return value /// Return value
virtual T value(const Values& values) const { T value(const Values& values) const override {
return function_(expression1_->value(values), boost::none); return function_(expression1_->value(values), boost::none);
} }
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { std::set<Key> keys() const override {
return expression1_->keys(); return expression1_->keys();
} }
/// Return dimensions for each argument /// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const { void dims(std::map<Key, int>& map) const override {
expression1_->dims(map); expression1_->dims(map);
} }
@ -307,8 +307,8 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const { ExecutionTraceStorage* ptr) const override {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0); assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
// Create a Record in the memory pointed to by ptr // Create a Record in the memory pointed to by ptr
@ -357,21 +357,21 @@ public:
} }
/// Print /// Print
virtual void print(const std::string& indent = "") const { void print(const std::string& indent = "") const override {
std::cout << indent << "BinaryExpression" << std::endl; std::cout << indent << "BinaryExpression" << std::endl;
expression1_->print(indent + " "); expression1_->print(indent + " ");
expression2_->print(indent + " "); expression2_->print(indent + " ");
} }
/// Return value /// Return value
virtual T value(const Values& values) const { T value(const Values& values) const override {
using boost::none; using boost::none;
return function_(expression1_->value(values), expression2_->value(values), return function_(expression1_->value(values), expression2_->value(values),
none, none); none, none);
} }
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { std::set<Key> keys() const override {
std::set<Key> keys = expression1_->keys(); std::set<Key> keys = expression1_->keys();
std::set<Key> myKeys = expression2_->keys(); std::set<Key> myKeys = expression2_->keys();
keys.insert(myKeys.begin(), myKeys.end()); keys.insert(myKeys.begin(), myKeys.end());
@ -379,7 +379,7 @@ public:
} }
/// Return dimensions for each argument /// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const { void dims(std::map<Key, int>& map) const override {
expression1_->dims(map); expression1_->dims(map);
expression2_->dims(map); expression2_->dims(map);
} }
@ -426,8 +426,8 @@ public:
}; };
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const { ExecutionTraceStorage* ptr) const override {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0); assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr); Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr);
trace.setFunction(record); trace.setFunction(record);
@ -464,7 +464,7 @@ public:
} }
/// Print /// Print
virtual void print(const std::string& indent = "") const { void print(const std::string& indent = "") const override {
std::cout << indent << "TernaryExpression" << std::endl; std::cout << indent << "TernaryExpression" << std::endl;
expression1_->print(indent + " "); expression1_->print(indent + " ");
expression2_->print(indent + " "); expression2_->print(indent + " ");
@ -472,14 +472,14 @@ public:
} }
/// Return value /// Return value
virtual T value(const Values& values) const { T value(const Values& values) const override {
using boost::none; using boost::none;
return function_(expression1_->value(values), expression2_->value(values), return function_(expression1_->value(values), expression2_->value(values),
expression3_->value(values), none, none, none); expression3_->value(values), none, none, none);
} }
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { std::set<Key> keys() const override {
std::set<Key> keys = expression1_->keys(); std::set<Key> keys = expression1_->keys();
std::set<Key> myKeys = expression2_->keys(); std::set<Key> myKeys = expression2_->keys();
keys.insert(myKeys.begin(), myKeys.end()); keys.insert(myKeys.begin(), myKeys.end());
@ -489,7 +489,7 @@ public:
} }
/// Return dimensions for each argument /// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const { void dims(std::map<Key, int>& map) const override {
expression1_->dims(map); expression1_->dims(map);
expression2_->dims(map); expression2_->dims(map);
expression3_->dims(map); expression3_->dims(map);
@ -544,8 +544,8 @@ public:
}; };
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const { ExecutionTraceStorage* ptr) const override {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0); assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr); Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr);
trace.setFunction(record); trace.setFunction(record);
@ -574,23 +574,23 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
virtual ~ScalarMultiplyNode() {} virtual ~ScalarMultiplyNode() {}
/// Print /// Print
virtual void print(const std::string& indent = "") const { void print(const std::string& indent = "") const override {
std::cout << indent << "ScalarMultiplyNode" << std::endl; std::cout << indent << "ScalarMultiplyNode" << std::endl;
expression_->print(indent + " "); expression_->print(indent + " ");
} }
/// Return value /// Return value
virtual T value(const Values& values) const { T value(const Values& values) const override {
return scalar_ * expression_->value(values); return scalar_ * expression_->value(values);
} }
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { std::set<Key> keys() const override {
return expression_->keys(); return expression_->keys();
} }
/// Return dimensions for each argument /// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const { void dims(std::map<Key, int>& map) const override {
expression_->dims(map); expression_->dims(map);
} }
@ -624,8 +624,8 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const { ExecutionTraceStorage* ptr) const override {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0); assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
Record* record = new (ptr) Record(); Record* record = new (ptr) Record();
ptr += upAligned(sizeof(Record)); ptr += upAligned(sizeof(Record));
@ -662,19 +662,19 @@ class BinarySumNode : public ExpressionNode<T> {
virtual ~BinarySumNode() {} virtual ~BinarySumNode() {}
/// Print /// Print
virtual void print(const std::string& indent = "") const { void print(const std::string& indent = "") const override {
std::cout << indent << "BinarySumNode" << std::endl; std::cout << indent << "BinarySumNode" << std::endl;
expression1_->print(indent + " "); expression1_->print(indent + " ");
expression2_->print(indent + " "); expression2_->print(indent + " ");
} }
/// Return value /// Return value
virtual T value(const Values& values) const { T value(const Values& values) const override {
return expression1_->value(values) + expression2_->value(values); return expression1_->value(values) + expression2_->value(values);
} }
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { std::set<Key> keys() const override {
std::set<Key> keys = expression1_->keys(); std::set<Key> keys = expression1_->keys();
std::set<Key> myKeys = expression2_->keys(); std::set<Key> myKeys = expression2_->keys();
keys.insert(myKeys.begin(), myKeys.end()); keys.insert(myKeys.begin(), myKeys.end());
@ -682,7 +682,7 @@ class BinarySumNode : public ExpressionNode<T> {
} }
/// Return dimensions for each argument /// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const { void dims(std::map<Key, int>& map) const override {
expression1_->dims(map); expression1_->dims(map);
expression2_->dims(map); expression2_->dims(map);
} }
@ -717,8 +717,8 @@ class BinarySumNode : public ExpressionNode<T> {
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const { ExecutionTraceStorage* ptr) const override {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0); assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
Record* record = new (ptr) Record(); Record* record = new (ptr) Record();
trace.setFunction(record); trace.setFunction(record);

6
gtsam/nonlinear/nonlinearExceptions.h
View File

@ -35,11 +35,11 @@ namespace gtsam {
KeyFormatter formatter_; KeyFormatter formatter_;
mutable std::string what_; mutable std::string what_;
public: public:
MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) throw() : MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) noexcept :
key_(key), formatter_(formatter) {} key_(key), formatter_(formatter) {}
virtual ~MarginalizeNonleafException() throw() {} virtual ~MarginalizeNonleafException() noexcept {}
Key key() const { return key_; } Key key() const { return key_; }
virtual const char* what() const throw() { const char* what() const noexcept override {
if(what_.empty()) if(what_.empty())
what_ = what_ =
"\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\ "\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\

4
gtsam/sam/BearingFactor.h
View File

@ -48,7 +48,7 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
} }
// Return measurement expression // Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const { Expression<T> expression(Key key1, Key key2) const override {
Expression<A1> a1_(key1); Expression<A1> a1_(key1);
Expression<A2> a2_(key2); Expression<A2> a2_(key2);
return Expression<T>(Bearing<A1, A2>(), a1_, a2_); return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
@ -56,7 +56,7 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
/// print /// print
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const { const KeyFormatter& kf = DefaultKeyFormatter) const override {
std::cout << s << "BearingFactor" << std::endl; std::cout << s << "BearingFactor" << std::endl;
Base::print(s, kf); Base::print(s, kf);
} }

8
gtsam/sam/BearingRangeFactor.h
View File

@ -55,20 +55,20 @@ class BearingRangeFactor
virtual ~BearingRangeFactor() {} virtual ~BearingRangeFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
// Return measurement expression // Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const { Expression<T> expression(Key key1, Key key2) const override {
return Expression<T>(T::Measure, Expression<A1>(key1), return Expression<T>(T::Measure, Expression<A1>(key1),
Expression<A2>(key2)); Expression<A2>(key2));
} }
/// print /// print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const { const KeyFormatter& kf = DefaultKeyFormatter) const override {
std::cout << s << "BearingRangeFactor" << std::endl; std::cout << s << "BearingRangeFactor" << std::endl;
Base::print(s, kf); Base::print(s, kf);
} }

12
gtsam/sam/RangeFactor.h
View File

@ -47,13 +47,13 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
// Return measurement expression // Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const { Expression<T> expression(Key key1, Key key2) const override {
Expression<A1> a1_(key1); Expression<A1> a1_(key1);
Expression<A2> a2_(key2); Expression<A2> a2_(key2);
return Expression<T>(Range<A1, A2>(), a1_, a2_); return Expression<T>(Range<A1, A2>(), a1_, a2_);
@ -61,7 +61,7 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
/// print /// print
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const { const KeyFormatter& kf = DefaultKeyFormatter) const override {
std::cout << s << "RangeFactor" << std::endl; std::cout << s << "RangeFactor" << std::endl;
Base::print(s, kf); Base::print(s, kf);
} }
@ -107,13 +107,13 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
virtual ~RangeFactorWithTransform() {} virtual ~RangeFactorWithTransform() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
// Return measurement expression // Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const { Expression<T> expression(Key key1, Key key2) const override {
Expression<A1> body_T_sensor__(body_T_sensor_); Expression<A1> body_T_sensor__(body_T_sensor_);
Expression<A1> nav_T_body_(key1); Expression<A1> nav_T_body_(key1);
Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_, Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
@ -124,7 +124,7 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
/** print contents */ /** print contents */
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "RangeFactorWithTransform" << std::endl; std::cout << s << "RangeFactorWithTransform" << std::endl;
this->body_T_sensor_.print(" sensor pose in body frame: "); this->body_T_sensor_.print(" sensor pose in body frame: ");
Base::print(s, keyFormatter); Base::print(s, keyFormatter);

14
gtsam/slam/AntiFactor.h
View File

@ -52,20 +52,20 @@ namespace gtsam {
virtual ~AntiFactor() {} virtual ~AntiFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** 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; std::cout << s << "AntiFactor version of:" << std::endl;
factor_->print(s, keyFormatter); factor_->print(s, keyFormatter);
} }
/** equals */ /** 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<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); 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, * For the AntiFactor, this will have the same magnitude of the original factor,
* but the opposite sign. * 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) */ /** 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. * 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. * 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 * 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 * 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. * effectively cancels the effect of the original factor on the factor graph.
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const { boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
// Generate the linearized factor from the contained nonlinear factor // Generate the linearized factor from the contained nonlinear factor
GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c); GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c);

10
gtsam/slam/BetweenFactor.h
View File

@ -63,14 +63,14 @@ namespace gtsam {
virtual ~BetweenFactor() {} virtual ~BetweenFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** 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(" std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->key2()) << ")\n";
@ -79,7 +79,7 @@ namespace gtsam {
} }
/** equals */ /** 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<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol); return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
} }
@ -87,8 +87,8 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 = Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x) T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR

8
gtsam/slam/BoundingConstraint.h
View File

@ -58,14 +58,14 @@ struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
boost::none) const = 0; boost::none) const = 0;
/** active when constraint *NOT* met */ /** 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 // note: still active at equality to avoid zigzagging
double x = value(c.at<X>(this->key())); double x = value(c.at<X>(this->key()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
} }
Vector evaluateError(const X& x, boost::optional<Matrix&> H = Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const override {
Matrix D; Matrix D;
double error = value(x, D) - threshold_; double error = value(x, D) - threshold_;
if (H) { if (H) {
@ -126,7 +126,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
boost::optional<Matrix&> H2 = boost::none) const = 0; boost::optional<Matrix&> H2 = boost::none) const = 0;
/** active when constraint *NOT* met */ /** 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 // note: still active at equality to avoid zigzagging
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2())); double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@ -134,7 +134,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
Vector evaluateError(const X1& x1, const X2& x2, Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const override {
Matrix D1, D2; Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_; double error = value(x1, x2, D1, D2) - threshold_;
if (H1) { if (H1) {

12
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -61,7 +61,7 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -69,18 +69,18 @@ public:
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** equals */ /** 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 */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
virtual Vector evaluateError(const Pose3& p1, const Pose3& p2, Vector evaluateError(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> Hp1 = boost::none, // boost::optional<Matrix&> Hp1 = boost::none, //
boost::optional<Matrix&> Hp2 = boost::none) const; boost::optional<Matrix&> Hp2 = boost::none) const override;
/** return the measured */ /** return the measured */
const EssentialMatrix& measured() const { const EssentialMatrix& measured() const {

24
gtsam/slam/EssentialMatrixFactor.h
View File

@ -58,14 +58,14 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/// print /// print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s); Base::print(s);
std::cout << " EssentialMatrixFactor with measurements\n (" std::cout << " EssentialMatrixFactor with measurements\n ("
<< vA_.transpose() << ")' and (" << vB_.transpose() << ")'" << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
@ -74,7 +74,7 @@ public:
/// vector of errors returns 1D vector /// vector of errors returns 1D vector
Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H = Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
boost::none) const { boost::none) const override {
Vector error(1); Vector error(1);
error << E.error(vA_, vB_, H); error << E.error(vA_, vB_, H);
return error; return error;
@ -131,14 +131,14 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/// print /// print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s); Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n (" std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.transpose() << ")' and (" << pn_.transpose() << dP1_.transpose() << ")' and (" << pn_.transpose()
@ -152,7 +152,7 @@ public:
*/ */
Vector evaluateError(const EssentialMatrix& E, const double& d, Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd = boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const { boost::none) const override {
// We have point x,y in image 1 // 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 // 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 /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/// print /// print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s); Base::print(s);
std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
} }
@ -269,7 +269,7 @@ public:
*/ */
Vector evaluateError(const EssentialMatrix& E, const double& d, Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd = boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const { boost::none) const override {
if (!DE) { if (!DE) {
// Convert E from body to camera frame // Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E; EssentialMatrix cameraE = cRb_ * E;

8
gtsam/slam/FrobeniusFactor.h
View File

@ -56,7 +56,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
/// Error is just Frobenius norm between Rot element and vectorized matrix M. /// Error is just Frobenius norm between Rot element and vectorized matrix M.
Vector evaluateError(const Rot& R, Vector evaluateError(const Rot& R,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const override {
return R.vec(H) - vecM_; // Jacobian is computed only when needed. return R.vec(H) - vecM_; // Jacobian is computed only when needed.
} }
}; };
@ -78,7 +78,7 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
/// Error is just Frobenius norm between rotation matrices. /// Error is just Frobenius norm between rotation matrices.
Vector evaluateError(const Rot& R1, const Rot& R2, Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const override {
Vector error = R2.vec(H2) - R1.vec(H1); Vector error = R2.vec(H2) - R1.vec(H1);
if (H1) *H1 = -*H1; if (H1) *H1 = -*H1;
return error; return error;
@ -110,7 +110,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
/// Error is Frobenius norm between R1*R12 and R2. /// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2, Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const override {
const Rot R2hat = R1.compose(R12_); const Rot R2hat = R1.compose(R12_);
Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat; Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
@ -139,7 +139,7 @@ class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn>
/// projects down from SO(p) to the Stiefel manifold of px3 matrices. /// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector evaluateError(const SOn& Q1, const SOn& Q2, Vector evaluateError(const SOn& Q1, const SOn& Q2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const; boost::optional<Matrix&> H2 = boost::none) const override;
}; };
} // namespace gtsam } // namespace gtsam

18
gtsam/slam/GeneralSFMFactor.h
View File

@ -96,7 +96,7 @@ public:
virtual ~GeneralSFMFactor() {} ///< destructor virtual ~GeneralSFMFactor() {} ///< destructor
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));} gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
@ -105,7 +105,7 @@ public:
* @param s optional string naming the factor * @param s optional string naming the factor
* @param keyFormatter optional formatter for printing out Symbols * @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); Base::print(s, keyFormatter);
traits<Point2>::Print(measured_, s + ".z"); traits<Point2>::Print(measured_, s + ".z");
} }
@ -113,14 +113,14 @@ public:
/** /**
* equals * 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<const This*>(&p); const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol); return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
} }
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point, Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
try { try {
return camera.project2(point,H1,H2) - measured_; return camera.project2(point,H1,H2) - measured_;
} }
@ -133,7 +133,7 @@ public:
} }
/// Linearize using fixed-size matrices /// Linearize using fixed-size matrices
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const { boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>(); if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
@ -230,7 +230,7 @@ public:
virtual ~GeneralSFMFactor2() {} ///< destructor virtual ~GeneralSFMFactor2() {} ///< destructor
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));} gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
@ -239,7 +239,7 @@ public:
* @param s optional string naming the factor * @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols * @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); Base::print(s, keyFormatter);
traits<Point2>::Print(measured_, s + ".z"); traits<Point2>::Print(measured_, s + ".z");
} }
@ -247,7 +247,7 @@ public:
/** /**
* equals * 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<const This*>(&p); const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol); return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
} }
@ -256,7 +256,7 @@ public:
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none, boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const boost::optional<Matrix&> H3=boost::none) const override
{ {
try { try {
Camera camera(pose3,calib); Camera camera(pose3,calib);

18
gtsam/slam/OrientedPlane3Factor.h
View File

@ -41,13 +41,13 @@ public:
} }
/// print /// print
virtual void print(const std::string& s = "OrientedPlane3Factor", void print(const std::string& s = "OrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// evaluateError /// evaluateError
virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const override {
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
H2); H2);
Vector err(3); Vector err(3);
@ -78,14 +78,14 @@ public:
} }
/// print /// print
virtual void print(const std::string& s = "OrientedPlane3DirectionPrior", void print(const std::string& s = "OrientedPlane3DirectionPrior",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// equals /// 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, Vector evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H = boost::none) const; boost::optional<Matrix&> H = boost::none) const override;
}; };
} // gtsam } // gtsam

8
gtsam/slam/PoseRotationPrior.h
View File

@ -50,7 +50,7 @@ public:
virtual ~PoseRotationPrior() {} virtual ~PoseRotationPrior() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
@ -60,19 +60,19 @@ public:
// testable // testable
/** equals specialized to this factor */ /** 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<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
} }
/** print contents */ /** 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); Base::print(s + "PoseRotationPrior", keyFormatter);
measured_.print("Measured Rotation"); measured_.print("Measured Rotation");
} }
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
const Rotation& newR = pose.rotation(); const Rotation& newR = pose.rotation();
if (H) { if (H) {
*H = Matrix::Zero(rDim, xDim); *H = Matrix::Zero(rDim, xDim);

8
gtsam/slam/PoseTranslationPrior.h
View File

@ -54,12 +54,12 @@ public:
const Translation& measured() const { return measured_; } const Translation& measured() const { return measured_; }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
const Translation& newTrans = pose.translation(); const Translation& newTrans = pose.translation();
const Rotation& R = pose.rotation(); const Rotation& R = pose.rotation();
const int tDim = traits<Translation>::GetDimension(newTrans); const int tDim = traits<Translation>::GetDimension(newTrans);
@ -74,13 +74,13 @@ public:
} }
/** equals specialized to this factor */ /** 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<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol); return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
} }
/** print contents */ /** 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); Base::print(s + "PoseTranslationPrior", keyFormatter);
traits<Translation>::Print(measured_, "Measured Translation"); traits<Translation>::Print(measured_, "Measured Translation");
} }

8
gtsam/slam/ProjectionFactor.h
View File

@ -100,7 +100,7 @@ namespace gtsam {
virtual ~GenericProjectionFactor() {} virtual ~GenericProjectionFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
@ -109,7 +109,7 @@ namespace gtsam {
* @param s optional string naming the factor * @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols * @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 = "; std::cout << s << "GenericProjectionFactor, z = ";
traits<Point2>::Print(measured_); traits<Point2>::Print(measured_);
if(this->body_P_sensor_) if(this->body_P_sensor_)
@ -118,7 +118,7 @@ namespace gtsam {
} }
/// equals /// 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<const This*>(&p); const This *e = dynamic_cast<const This*>(&p);
return e return e
&& Base::equals(p, tol) && Base::equals(p, tol)
@ -129,7 +129,7 @@ namespace gtsam {
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Point3& point, Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
try { try {
if(body_P_sensor_) { if(body_P_sensor_) {
if(H1) { if(H1) {

10
gtsam/slam/ReferenceFrameFactor.h
View File

@ -89,23 +89,23 @@ public:
virtual ~ReferenceFrameFactor(){} virtual ~ReferenceFrameFactor(){}
virtual NonlinearFactor::shared_ptr clone() const { NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>( return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); } NonlinearFactor::shared_ptr(new This(*this))); }
/** Combined cost and derivative function using boost::optional */ /** 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<Matrix&> Dforeign = boost::none, boost::optional<Matrix&> Dforeign = boost::none,
boost::optional<Matrix&> Dtrans = boost::none, boost::optional<Matrix&> Dtrans = boost::none,
boost::optional<Matrix&> Dlocal = boost::none) const { boost::optional<Matrix&> Dlocal = boost::none) const override {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign); Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal) if (Dlocal)
*Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension); *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
return traits<Point>::Local(local,newlocal); return traits<Point>::Local(local,newlocal);
} }
virtual void print(const std::string& s="", void print(const std::string& s="",
const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": ReferenceFrameFactor(" std::cout << s << ": ReferenceFrameFactor("
<< "Global: " << keyFormatter(this->key1()) << "," << "Global: " << keyFormatter(this->key1()) << ","
<< " Transform: " << keyFormatter(this->key2()) << "," << " Transform: " << keyFormatter(this->key2()) << ","

16
gtsam/slam/RotateFactor.h
View File

@ -36,13 +36,13 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// print /// print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s); Base::print(s);
std::cout << "RotateFactor:]\n"; std::cout << "RotateFactor:]\n";
std::cout << "p: " << p_.transpose() << std::endl; std::cout << "p: " << p_.transpose() << std::endl;
@ -51,7 +51,7 @@ public:
/// vector of errors returns 2D vector /// vector of errors returns 2D vector
Vector evaluateError(const Rot3& R, Vector evaluateError(const Rot3& R,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const override {
// predict p_ as q = R*z_, derivative H will be filled if not none // predict p_ as q = R*z_, derivative H will be filled if not none
Point3 q = R.rotate(z_,H); Point3 q = R.rotate(z_,H);
// error is just difference, and note derivative of that wrpt q is I3 // 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 /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// print /// print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s); Base::print(s);
std::cout << "RotateDirectionsFactor:" << std::endl; std::cout << "RotateDirectionsFactor:" << std::endl;
i_p_.print("p"); i_p_.print("p");
@ -102,7 +102,7 @@ public:
} }
/// vector of errors returns 2D vector /// vector of errors returns 2D vector
Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
Unit3 i_q = iRc * c_z_; Unit3 i_q = iRc * c_z_;
Vector error = i_p_.error(i_q, H); Vector error = i_p_.error(i_q, H);
if (H) { if (H) {

6
gtsam/slam/SmartFactorBase.h
View File

@ -150,7 +150,7 @@ protected:
} }
/// get the dimension (number of rows!) of the factor /// get the dimension (number of rows!) of the factor
virtual size_t dim() const { size_t dim() const override {
return ZDim * this->measured_.size(); return ZDim * this->measured_.size();
} }
@ -173,7 +173,7 @@ protected:
* @param keyFormatter optional formatter useful for printing Symbols * @param keyFormatter optional formatter useful for printing Symbols
*/ */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
std::cout << s << "SmartFactorBase, z = \n"; std::cout << s << "SmartFactorBase, z = \n";
for (size_t k = 0; k < measured_.size(); ++k) { for (size_t k = 0; k < measured_.size(); ++k) {
std::cout << "measurement, p = " << measured_[k] << "\t"; std::cout << "measurement, p = " << measured_[k] << "\t";
@ -185,7 +185,7 @@ protected:
} }
/// equals /// 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<const This*>(&p); const This *e = dynamic_cast<const This*>(&p);
bool areMeasurementsEqual = true; bool areMeasurementsEqual = true;

10
gtsam/slam/SmartProjectionFactor.h
View File

@ -99,7 +99,7 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols * @param keyFormatter optional formatter useful for printing Symbols
*/ */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionFactor\n"; std::cout << s << "SmartProjectionFactor\n";
std::cout << "linearizationMode:\n" << params_.linearizationMode std::cout << "linearizationMode:\n" << params_.linearizationMode
<< std::endl; << std::endl;
@ -110,7 +110,7 @@ public:
} }
/// equals /// 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<const This*>(&p); const This *e = dynamic_cast<const This*>(&p);
return e && params_.linearizationMode == e->params_.linearizationMode return e && params_.linearizationMode == e->params_.linearizationMode
&& Base::equals(p, tol); && Base::equals(p, tol);
@ -305,8 +305,8 @@ public:
} }
/// linearize /// linearize
virtual boost::shared_ptr<GaussianFactor> linearize( boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const { const Values& values) const override {
return linearizeDamped(values); return linearizeDamped(values);
} }
@ -409,7 +409,7 @@ public:
} }
/// Calculate total reprojection error /// Calculate total reprojection error
virtual double error(const Values& values) const { double error(const Values& values) const override {
if (this->active(values)) { if (this->active(values)) {
return totalReprojectionError(Base::cameras(values)); return totalReprojectionError(Base::cameras(values));
} else { // else of active flag } else { // else of active flag

8
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -103,13 +103,13 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols * @param keyFormatter optional formatter useful for printing Symbols
*/ */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionPoseFactor, z = \n "; std::cout << s << "SmartProjectionPoseFactor, z = \n ";
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }
/// equals /// 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<const This*>(&p); const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol); return e && Base::equals(p, tol);
} }
@ -117,7 +117,7 @@ public:
/** /**
* error calculates the error of the factor. * 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)) { if (this->active(values)) {
return this->totalReprojectionError(cameras(values)); return this->totalReprojectionError(cameras(values));
} else { // else of active flag } else { // else of active flag
@ -136,7 +136,7 @@ public:
* to keys involved in this factor * to keys involved in this factor
* @return vector of Values * @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; typename Base::Cameras cameras;
for (const Key& k : this->keys_) { for (const Key& k : this->keys_) {
const Pose3 world_P_sensor_k = const Pose3 world_P_sensor_k =

8
gtsam/slam/StereoFactor.h
View File

@ -91,7 +91,7 @@ public:
virtual ~GenericStereoFactor() {} virtual ~GenericStereoFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
@ -100,7 +100,7 @@ public:
* @param s optional string naming the factor * @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols * @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); Base::print(s, keyFormatter);
measured_.print(s + ".z"); measured_.print(s + ".z");
if(this->body_P_sensor_) if(this->body_P_sensor_)
@ -110,7 +110,7 @@ public:
/** /**
* equals * 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<const GenericStereoFactor*> (&f); const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
return e return e
&& Base::equals(f) && Base::equals(f)
@ -120,7 +120,7 @@ public:
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point, Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
try { try {
if(body_P_sensor_) { if(body_P_sensor_) {
if(H1) { if(H1) {

10
gtsam/slam/TriangulationFactor.h
View File

@ -90,7 +90,7 @@ public:
} }
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -101,7 +101,7 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols * @param keyFormatter optional formatter useful for printing Symbols
*/ */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
std::cout << s << "TriangulationFactor,"; std::cout << s << "TriangulationFactor,";
camera_.print("camera"); camera_.print("camera");
traits<Measurement>::Print(measured_, "z"); traits<Measurement>::Print(measured_, "z");
@ -109,7 +109,7 @@ public:
} }
/// equals /// 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<const This*>(&p); const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol) return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
&& traits<Measurement>::Equals(this->measured_, e->measured_, tol); && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
@ -117,7 +117,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 = Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const override {
try { try {
return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2)); return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
@ -143,7 +143,7 @@ public:
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * \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$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!this->active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();

12
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -41,9 +41,9 @@ public:
boost::optional<Pose3> body_P_sensor = boost::none, boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10) size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
virtual double error(const Values& values) const { return 0.0; } double error(const Values& values) const override { return 0.0; }
virtual boost::shared_ptr<GaussianFactor> linearize( boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const { const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor()); return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
} }
}; };
@ -105,11 +105,11 @@ public:
StereoFactor() {} StereoFactor() {}
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
} }
virtual double error(const Values& values) const { double error(const Values& values) const override {
return 0.0; return 0.0;
} }
virtual boost::shared_ptr<GaussianFactor> linearize( boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const { const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor()); return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
} }
}; };

2
gtsam/symbolic/SymbolicConditional.h
View File

@ -105,7 +105,7 @@ namespace gtsam {
/// @name Testable /// @name Testable
/** Print with optional formatter */ /** 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 */ /** Check equality */
bool equals(const This& c, double tol = 1e-9) const; bool equals(const This& c, double tol = 1e-9) const;

18
gtsam_unstable/discrete/AllDiff.h
View File

@ -34,11 +34,11 @@ namespace gtsam {
AllDiff(const DiscreteKeys& dkeys); AllDiff(const DiscreteKeys& dkeys);
// print // print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// equals /// equals
bool equals(const DiscreteFactor& other, double tol) const { bool equals(const DiscreteFactor& other, double tol) const override {
if(!dynamic_cast<const AllDiff*>(&other)) if(!dynamic_cast<const AllDiff*>(&other))
return false; return false;
else { else {
@ -50,13 +50,13 @@ namespace gtsam {
} }
/// Calculate value = expensive ! /// 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 ! /// Convert into a decisiontree, can be *very* expensive !
virtual DecisionTreeFactor toDecisionTreeFactor() const; DecisionTreeFactor toDecisionTreeFactor() const override;
/// Multiply into a decisiontree /// Multiply into a decisiontree
virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
/* /*
* Ensure Arc-consistency * Ensure Arc-consistency
@ -65,13 +65,13 @@ namespace gtsam {
* @param j domain to be checked * @param j domain to be checked
* @param domains all other domains * @param domains all other domains
*/ */
bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const; bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override;
/// Partially apply known values /// 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 /// Partially apply known values, domain version
virtual Constraint::shared_ptr partiallyApply(const std::vector<Domain>&) const; Constraint::shared_ptr partiallyApply(const std::vector<Domain>&) const override;
}; };
} // namespace gtsam } // namespace gtsam

20
gtsam_unstable/discrete/BinaryAllDiff.h
View File

@ -33,14 +33,14 @@ namespace gtsam {
} }
// print // print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const { const KeyFormatter& formatter = DefaultKeyFormatter) const override {
std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
<< formatter(keys_[1]) << std::endl; << formatter(keys_[1]) << std::endl;
} }
/// equals /// equals
bool equals(const DiscreteFactor& other, double tol) const { bool equals(const DiscreteFactor& other, double tol) const override {
if(!dynamic_cast<const BinaryAllDiff*>(&other)) if(!dynamic_cast<const BinaryAllDiff*>(&other))
return false; return false;
else { else {
@ -50,12 +50,12 @@ namespace gtsam {
} }
/// Calculate value /// 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])); return (double) (values.at(keys_[0]) != values.at(keys_[1]));
} }
/// Convert into a decisiontree /// Convert into a decisiontree
virtual DecisionTreeFactor toDecisionTreeFactor() const { DecisionTreeFactor toDecisionTreeFactor() const override {
DiscreteKeys keys; DiscreteKeys keys;
keys.push_back(DiscreteKey(keys_[0],cardinality0_)); keys.push_back(DiscreteKey(keys_[0],cardinality0_));
keys.push_back(DiscreteKey(keys_[1],cardinality1_)); keys.push_back(DiscreteKey(keys_[1],cardinality1_));
@ -68,7 +68,7 @@ namespace gtsam {
} }
/// Multiply into a decisiontree /// 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? // TODO: can we do this more efficiently?
return toDecisionTreeFactor() * f; return toDecisionTreeFactor() * f;
} }
@ -79,20 +79,20 @@ namespace gtsam {
* @param domains all other domains * @param domains all other domains
*/ */
/// ///
bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const { bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override {
// throw std::runtime_error( // throw std::runtime_error(
// "BinaryAllDiff::ensureArcConsistency not implemented"); // "BinaryAllDiff::ensureArcConsistency not implemented");
return false; return false;
} }
/// Partially apply known values /// 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"); throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
} }
/// Partially apply known values, domain version /// Partially apply known values, domain version
virtual Constraint::shared_ptr partiallyApply( Constraint::shared_ptr partiallyApply(
const std::vector<Domain>&) const { const std::vector<Domain>&) const override {
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
} }
}; };

21
gtsam_unstable/discrete/Domain.h
View File

@ -66,11 +66,11 @@ namespace gtsam {
} }
// print // print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// equals /// equals
bool equals(const DiscreteFactor& other, double tol) const { bool equals(const DiscreteFactor& other, double tol) const override {
if(!dynamic_cast<const Domain*>(&other)) if(!dynamic_cast<const Domain*>(&other))
return false; return false;
else { else {
@ -84,20 +84,20 @@ namespace gtsam {
} }
/// Calculate value /// Calculate value
virtual double operator()(const Values& values) const; double operator()(const Values& values) const override;
/// Convert into a decisiontree /// Convert into a decisiontree
virtual DecisionTreeFactor toDecisionTreeFactor() const; DecisionTreeFactor toDecisionTreeFactor() const override;
/// Multiply into a decisiontree /// Multiply into a decisiontree
virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
/* /*
* Ensure Arc-consistency * Ensure Arc-consistency
* @param j domain to be checked * @param j domain to be checked
* @param domains all other domains * @param domains all other domains
*/ */
bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const; bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override;
/** /**
* Check for a value in domain that does not occur in any other connected domain. * 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<Domain>& domains); bool checkAllDiff(const KeyVector keys, std::vector<Domain>& domains);
/// Partially apply known values /// Partially apply known values
virtual Constraint::shared_ptr partiallyApply( Constraint::shared_ptr partiallyApply(const Values& values) const override;
const Values& values) const;
/// Partially apply known values, domain version /// Partially apply known values, domain version
virtual Constraint::shared_ptr partiallyApply( Constraint::shared_ptr partiallyApply(
const std::vector<Domain>& domains) const; const std::vector<Domain>& domains) const override;
}; };
} // namespace gtsam } // namespace gtsam

21
gtsam_unstable/discrete/SingleValue.h
View File

@ -42,11 +42,11 @@ namespace gtsam {
} }
// print // print
virtual void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// equals /// equals
bool equals(const DiscreteFactor& other, double tol) const { bool equals(const DiscreteFactor& other, double tol) const override {
if(!dynamic_cast<const SingleValue*>(&other)) if(!dynamic_cast<const SingleValue*>(&other))
return false; return false;
else { else {
@ -56,28 +56,27 @@ namespace gtsam {
} }
/// Calculate value /// Calculate value
virtual double operator()(const Values& values) const; double operator()(const Values& values) const override;
/// Convert into a decisiontree /// Convert into a decisiontree
virtual DecisionTreeFactor toDecisionTreeFactor() const; DecisionTreeFactor toDecisionTreeFactor() const override;
/// Multiply into a decisiontree /// Multiply into a decisiontree
virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
/* /*
* Ensure Arc-consistency * Ensure Arc-consistency
* @param j domain to be checked * @param j domain to be checked
* @param domains all other domains * @param domains all other domains
*/ */
bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const; bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override;
/// Partially apply known values /// Partially apply known values
virtual Constraint::shared_ptr partiallyApply( Constraint::shared_ptr partiallyApply(const Values& values) const override;
const Values& values) const;
/// Partially apply known values, domain version /// Partially apply known values, domain version
virtual Constraint::shared_ptr partiallyApply( Constraint::shared_ptr partiallyApply(
const std::vector<Domain>& domains) const; const std::vector<Domain>& domains) const override;
}; };
} // namespace gtsam } // namespace gtsam

10
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -51,12 +51,12 @@ public:
virtual ~FullIMUFactor() {} virtual ~FullIMUFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */ /** 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<const This*>(&e); const This* const f = dynamic_cast<const This*>(&e);
return f && Base::equals(e) && return f && Base::equals(e) &&
equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(accel_, f->accel_, tol) &&
@ -64,7 +64,7 @@ public:
std::abs(dt_ - f->dt_) < tol; 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; std::string a = "FullIMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print((Vector)accel_, "accel"); gtsam::print((Vector)accel_, "accel");
@ -81,9 +81,9 @@ public:
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
* z - h(x1,x2) * z - h(x1,x2)
*/ */
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const override {
Vector9 z; Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang 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 z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang

10
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -44,12 +44,12 @@ public:
virtual ~IMUFactor() {} virtual ~IMUFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */ /** 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<const This*>(&e); const This* const f = dynamic_cast<const This*>(&e);
return f && Base::equals(e) && return f && Base::equals(e) &&
equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(accel_, f->accel_, tol) &&
@ -57,7 +57,7 @@ public:
std::abs(dt_ - f->dt_) < tol; 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; std::string a = "IMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print((Vector)accel_, "accel"); gtsam::print((Vector)accel_, "accel");
@ -74,9 +74,9 @@ public:
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
* z - h(x1,x2) * z - h(x1,x2)
*/ */
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const override {
const Vector6 meas = z(); const Vector6 meas = z();
if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);

16
gtsam_unstable/dynamics/Pendulum.h
View File

@ -40,7 +40,7 @@ public:
: Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {} : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
@ -48,7 +48,7 @@ public:
Vector evaluateError(const double& qk1, const double& qk, const double& v, Vector evaluateError(const double& qk1, const double& qk, const double& v,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = -Matrix::Identity(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = 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) {} : Base(noiseModel::Constrained::All(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
@ -96,7 +96,7 @@ public:
Vector evaluateError(const double & vk1, const double & vk, const double & q, Vector evaluateError(const double & vk1, const double & vk, const double & q,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = -Matrix::Identity(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = 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) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
@ -147,7 +147,7 @@ public:
Vector evaluateError(const double & pk, const double & qk, const double & qk1, Vector evaluateError(const double & pk, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
@ -195,7 +195,7 @@ public:
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
@ -203,7 +203,7 @@ public:
Vector evaluateError(const double & pk1, const double & qk, const double & qk1, Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;

8
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -36,7 +36,7 @@ public:
virtual ~Reconstruction() {} virtual ~Reconstruction() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); } gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
@ -44,7 +44,7 @@ public:
Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik, Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const override {
Matrix6 D_exphxi_xi; Matrix6 D_exphxi_xi;
Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0); Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0);
@ -98,7 +98,7 @@ public:
virtual ~DiscreteEulerPoincareHelicopter() {} virtual ~DiscreteEulerPoincareHelicopter() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); } gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); }
@ -110,7 +110,7 @@ public:
Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const override {
Vector muk = Inertia_*xik; Vector muk = Inertia_*xik;
Vector muk_1 = Inertia_*xik_1; Vector muk_1 = Inertia_*xik_1;

8
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -73,16 +73,16 @@ public:
virtual ~VelocityConstraint() {} virtual ~VelocityConstraint() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); } gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); }
/** /**
* Calculates the error for trapezoidal model given * 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<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const { boost::optional<gtsam::Matrix&> H2=boost::none) const override {
if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>( if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>( if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
@ -90,7 +90,7 @@ public:
return evaluateError_(x1, x2, dt_, integration_mode_); 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; std::string a = "VelocityConstraint: " + s;
Base::print(a, formatter); Base::print(a, formatter);
switch(integration_mode_) { switch(integration_mode_) {

4
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -31,7 +31,7 @@ public:
virtual ~VelocityConstraint3() {} virtual ~VelocityConstraint3() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
@ -39,7 +39,7 @@ public:
Vector evaluateError(const double& x1, const double& x2, const double& v, Vector evaluateError(const double& x1, const double& x2, const double& v,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = Matrix::Identity(p,p); if (H1) *H1 = Matrix::Identity(p,p);
if (H2) *H2 = -Matrix::Identity(p,p); if (H2) *H2 = -Matrix::Identity(p,p);

4
gtsam_unstable/linear/InfeasibleInitialValues.h
View File

@ -29,10 +29,10 @@ public:
InfeasibleInitialValues() { InfeasibleInitialValues() {
} }
virtual ~InfeasibleInitialValues() throw () { virtual ~InfeasibleInitialValues() noexcept {
} }
virtual const char *what() const throw () { const char *what() const noexcept override {
if (description_.empty()) if (description_.empty())
description_ = description_ =
"An infeasible initial value was provided for the solver.\n"; "An infeasible initial value was provided for the solver.\n";

4
gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
View File

@ -25,10 +25,10 @@ class InfeasibleOrUnboundedProblem: public ThreadsafeException<
public: public:
InfeasibleOrUnboundedProblem() { InfeasibleOrUnboundedProblem() {
} }
virtual ~InfeasibleOrUnboundedProblem() throw () { virtual ~InfeasibleOrUnboundedProblem() noexcept {
} }
virtual const char* what() const throw () { const char* what() const noexcept override {
if (description_.empty()) if (description_.empty())
description_ = "The problem is either infeasible or unbounded.\n"; description_ = "The problem is either infeasible or unbounded.\n";
return description_.c_str(); return description_.c_str();

10
gtsam_unstable/linear/LinearCost.h
View File

@ -88,18 +88,18 @@ public:
} }
/** equals */ /** 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); return Base::equals(lf, tol);
} }
/** print */ /** print */
virtual void print(const std::string& s = "", const KeyFormatter& formatter = void print(const std::string& s = "", const KeyFormatter& formatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
Base::print(s + " LinearCost: ", formatter); Base::print(s + " LinearCost: ", formatter);
} }
/** Clone this LinearCost */ /** Clone this LinearCost */
virtual GaussianFactor::shared_ptr clone() const { GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast < GaussianFactor return boost::static_pointer_cast < GaussianFactor
> (boost::make_shared < LinearCost > (*this)); > (boost::make_shared < LinearCost > (*this));
} }
@ -110,7 +110,7 @@ public:
} }
/** Special error for single-valued inequality constraints. */ /** 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]; return error_vector(c)[0];
} }
}; };

10
gtsam_unstable/linear/LinearEquality.h
View File

@ -89,18 +89,18 @@ public:
} }
/** equals */ /** 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); return Base::equals(lf, tol);
} }
/** print */ /** print */
virtual void print(const std::string& s = "", const KeyFormatter& formatter = void print(const std::string& s = "", const KeyFormatter& formatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
Base::print(s, formatter); Base::print(s, formatter);
} }
/** Clone this LinearEquality */ /** Clone this LinearEquality */
virtual GaussianFactor::shared_ptr clone() const { GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast < GaussianFactor return boost::static_pointer_cast < GaussianFactor
> (boost::make_shared < LinearEquality > (*this)); > (boost::make_shared < LinearEquality > (*this));
} }
@ -124,7 +124,7 @@ public:
* I think it should be zero, as this function is meant for objective cost. * I think it should be zero, as this function is meant for objective cost.
* But the name "error" can be misleading. * But the name "error" can be misleading.
* TODO: confirm with Frank!! */ * TODO: confirm with Frank!! */
virtual double error(const VectorValues& c) const { double error(const VectorValues& c) const override {
return 0.0; return 0.0;
} }

10
gtsam_unstable/linear/LinearInequality.h
View File

@ -100,13 +100,13 @@ public:
} }
/** equals */ /** 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); return Base::equals(lf, tol);
} }
/** print */ /** print */
virtual void print(const std::string& s = "", const KeyFormatter& formatter = void print(const std::string& s = "", const KeyFormatter& formatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
if (active()) if (active())
Base::print(s + " Active", formatter); Base::print(s + " Active", formatter);
else else
@ -114,7 +114,7 @@ public:
} }
/** Clone this LinearInequality */ /** Clone this LinearInequality */
virtual GaussianFactor::shared_ptr clone() const { GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast < GaussianFactor return boost::static_pointer_cast < GaussianFactor
> (boost::make_shared < LinearInequality > (*this)); > (boost::make_shared < LinearInequality > (*this));
} }
@ -145,7 +145,7 @@ public:
} }
/** Special error for single-valued inequality constraints. */ /** 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]; return error_vector(c)[0];
} }

4
gtsam_unstable/linear/QPSParserException.h
View File

@ -25,10 +25,10 @@ public:
QPSParserException() { QPSParserException() {
} }
virtual ~QPSParserException() throw () { virtual ~QPSParserException() noexcept {
} }
virtual const char *what() const throw () { const char *what() const noexcept override {
if (description_.empty()) if (description_.empty())
description_ = "There is a problem parsing the QPS file.\n"; description_ = "There is a problem parsing the QPS file.\n";
return description_.c_str(); return description_.c_str();

4
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -41,10 +41,10 @@ public:
virtual ~BatchFixedLagSmoother() { }; virtual ~BatchFixedLagSmoother() { };
/** Print the factor for debugging and testing (implementing Testable) */ /** 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 */ /** 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. */ /** Add new factors, updating the solution and relinearizing as needed. */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),

14
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -67,10 +67,10 @@ public:
virtual ~ConcurrentBatchFilter() {}; virtual ~ConcurrentBatchFilter() {};
/** Implement a GTSAM standard 'print' function */ /** 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 */ /** 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 */ /** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const { const NonlinearFactorGraph& getFactors() const {
@ -130,7 +130,7 @@ public:
* Perform any required operations before the synchronization process starts. * Perform any required operations before the synchronization process starts.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void presync(); virtual void presync() override;
/** /**
* Populate the provided containers with factors that constitute the filter branch summarization * 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 summarizedFactors The summarized factors for the filter branch
* @param rootValues The linearization points of the root clique variables * @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 * 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 smootherFactors The new factors to be added to the smoother
* @param smootherValues The linearization points of any new variables * @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. * Apply the updated version of the smoother branch summarized factors.
* *
* @param summarizedFactors An 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. * Perform any required operations after the synchronization process finishes.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void postsync(); virtual void postsync() override;
protected: protected:

14
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
View File

@ -60,10 +60,10 @@ public:
virtual ~ConcurrentBatchSmoother() {}; virtual ~ConcurrentBatchSmoother() {};
/** Implement a GTSAM standard 'print' function */ /** 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 */ /** 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 */ /** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const { const NonlinearFactorGraph& getFactors() const {
@ -124,7 +124,7 @@ public:
* Perform any required operations before the synchronization process starts. * Perform any required operations before the synchronization process starts.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void presync(); virtual void presync() override;
/** /**
* Populate the provided containers with factors that constitute the smoother branch summarization * 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 * @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 * 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 summarizedFactors An updated version of the filter branch summarized factors
* @param rootValues The linearization point of the root variables * @param rootValues The linearization point of the root variables
*/ */
virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues); const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
/** /**
* Perform any required operations after the synchronization process finishes. * Perform any required operations after the synchronization process finishes.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void postsync(); virtual void postsync() override;
protected: protected:

14
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
View File

@ -67,10 +67,10 @@ public:
virtual ~ConcurrentIncrementalFilter() {}; virtual ~ConcurrentIncrementalFilter() {};
/** Implement a GTSAM standard 'print' function */ /** 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 */ /** 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 */ /** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const { const NonlinearFactorGraph& getFactors() const {
@ -130,7 +130,7 @@ public:
* Perform any required operations before the synchronization process starts. * Perform any required operations before the synchronization process starts.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void presync(); void presync() override;
/** /**
* Populate the provided containers with factors that constitute the filter branch summarization * 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 summarizedFactors The summarized factors for the filter branch
* @param rootValues The linearization points of the root clique variables * @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 * 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 smootherFactors The new factors to be added to the smoother
* @param smootherValues The linearization points of any new variables * @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. * Apply the updated version of the smoother branch summarized factors.
* *
* @param summarizedFactors An 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. * Perform any required operations after the synchronization process finishes.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void postsync(); void postsync() override;
protected: protected:

14
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h
View File

@ -57,10 +57,10 @@ public:
virtual ~ConcurrentIncrementalSmoother() {}; virtual ~ConcurrentIncrementalSmoother() {};
/** Implement a GTSAM standard 'print' function */ /** 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 */ /** 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 */ /** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const { const NonlinearFactorGraph& getFactors() const {
@ -115,7 +115,7 @@ public:
* Perform any required operations before the synchronization process starts. * Perform any required operations before the synchronization process starts.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void presync(); void presync() override;
/** /**
* Populate the provided containers with factors that constitute the smoother branch summarization * 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 * @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 * 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 summarizedFactors An updated version of the filter branch summarized factors
* @param rootValues The linearization point of the root variables * @param rootValues The linearization point of the root variables
*/ */
virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues); const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
/** /**
* Perform any required operations after the synchronization process finishes. * Perform any required operations after the synchronization process finishes.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void postsync(); void postsync() override;
protected: protected:

24
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -112,17 +112,17 @@ public:
virtual ~LinearizedJacobianFactor() {} virtual ~LinearizedJacobianFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// Testable // Testable
/** print function */ /** 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 */ /** 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 // access functions
const constBVector b() const { return Ab_(size()).col(0); } 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()); } 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) */ /** 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 */ /** Calculate the error of the factor */
double error(const Values& c) const; double error(const Values& c) const override;
/** /**
* linearize to a GaussianFactor * linearize to a GaussianFactor
* Reimplemented from NoiseModelFactor to directly copy out the * Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual * matrices and only update the RHS b with an updated residual
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const; boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
/** (A*x-b) */ /** (A*x-b) */
Vector error_vector(const Values& c) const; Vector error_vector(const Values& c) const;
@ -202,17 +202,17 @@ public:
virtual ~LinearizedHessianFactor() {} virtual ~LinearizedHessianFactor() {}
/// @return a deep copy of this factor /// @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>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// Testable // Testable
/** print function */ /** 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 */ /** 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$ as described above
* @return The constant term \f$ f \f$ * @return The constant term \f$ f \f$
@ -261,17 +261,17 @@ public:
} }
/** get the dimension of the factor (number of rows on linearization) */ /** 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 */ /** Calculate the error of the factor */
double error(const Values& c) const; double error(const Values& c) const override;
/** /**
* linearize to a GaussianFactor * linearize to a GaussianFactor
* Reimplemented from NoiseModelFactor to directly copy out the * Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual * matrices and only update the RHS b with an updated residual
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const; boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
private: private:
/** Serialization function */ /** Serialization function */

8
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -84,8 +84,8 @@ public:
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << "," std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
<< keyFormatter(key2_) << ")\n"; << keyFormatter(key2_) << ")\n";
measured_.print(" measured: "); measured_.print(" measured: ");
@ -97,7 +97,7 @@ public:
} }
/** equals */ /** 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<const This*>(&f); const This *t = dynamic_cast<const This*>(&f);
if (t && Base::equals(f)) if (t && Base::equals(f))
@ -408,7 +408,7 @@ public:
return 2; return 2;
} }
virtual size_t dim() const { size_t dim() const override {
return model_inlier_->R().rows() + model_inlier_->R().cols(); return model_inlier_->R().rows() + model_inlier_->R().cols();
} }

6
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -55,7 +55,7 @@ namespace gtsam {
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** 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(" std::cout << s << "BiasedGPSFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n" << keyFormatter(this->key2()) << ")\n"
@ -64,7 +64,7 @@ namespace gtsam {
} }
/** equals */ /** 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<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol); return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
} }
@ -74,7 +74,7 @@ namespace gtsam {
/** vector of errors */ /** vector of errors */
Vector evaluateError(const Pose3& pose, const Point3& bias, Vector evaluateError(const Pose3& pose, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const override {
if (H1 || H2){ if (H1 || H2){
H1->resize(3,6); // jacobian wrt pose H1->resize(3,6); // jacobian wrt pose

12
gtsam_unstable/slam/DummyFactor.h
View File

@ -34,10 +34,10 @@ public:
// testable // testable
/** print */ /** 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 */ /** 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 // access
@ -48,13 +48,13 @@ public:
/** /**
* Calculate the error of the factor - zero for dummy factors * 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) */ /** 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 */ /** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& c) const; boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
/** /**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow * 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. * 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>( return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new DummyFactor(*this))); NonlinearFactor::shared_ptr(new DummyFactor(*this)));
} }

6
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -133,7 +133,7 @@ public:
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** 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 << "(" std::cout << s << "("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key2()) << ","
@ -153,7 +153,7 @@ public:
} }
/** equals */ /** 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<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) return e != nullptr && Base::equals(*e, tol)
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
@ -302,7 +302,7 @@ public:
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const { boost::optional<Matrix&> H5 = boost::none) const override {
// TODO: Write analytical derivative calculations // TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1 // Jacobian w.r.t. Pose1

Some files were not shown because too many files have changed in this diff Show More