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

Merge branch 'develop' into feature/values-print

release/4.3a0
Varun Agrawal 2020-07-28 08:59:35 -05:00
parent b8f9ab1d87 c985701fdc
commit 9bb07851c3
167 changed files with 1094 additions and 908 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.travis.yml
View File

@ -1,6 +1,5 @@
language: cpp
cache: ccache
sudo: required
dist: xenial
addons:
@ -33,7 +32,7 @@ stages:
env:
global:
- MAKEFLAGS="-j2"
- MAKEFLAGS="-j3"
- CCACHE_SLOPPINESS=pch_defines,time_macros
# Compile stage without building examples/tests to populate the caches.

6
CMakeLists.txt
View File

@ -10,7 +10,7 @@ endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 2)
set (GTSAM_VERSION_PATCH 3)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@ -68,8 +68,8 @@ if(GTSAM_UNSTABLE_AVAILABLE)
endif()
option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF)

4
CppUnitLite/Test.h
View File

@ -64,7 +64,7 @@ protected:
class testGroup##testName##Test : public Test \
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
virtual ~testGroup##testName##Test () {};\
void run (TestResult& result_);} \
void run (TestResult& result_) override;} \
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)
@ -82,7 +82,7 @@ protected:
class testGroup##testName##Test : public Test \
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \
virtual ~testGroup##testName##Test () {};\
void run (TestResult& result_);} \
void run (TestResult& result_) override;} \
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)

2
appveyor.yml
View File

@ -1,5 +1,5 @@
# version format
version: 4.0.2-{branch}-build{build}
version: 4.0.3-{branch}-build{build}
os: Visual Studio 2019

20
cmake/GtsamBuildTypes.cmake
View File

@ -104,8 +104,24 @@ if(MSVC)
set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
else()
# Common to all configurations, next for each configuration:
# "-fPIC" is to ensure proper code generation for shared libraries
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall -fPIC CACHE STRING "(User editable) Private compiler flags for all configurations.")
if (
((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR
(CMAKE_CXX_COMPILER_ID MATCHES "GNU")
)
set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
endif()
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
-Wall # Enable common warnings
-fPIC # ensure proper code generation for shared libraries
$<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address
-Wreturn-type -Werror=return-type # Error on missing return()
-Wformat -Werror=format-security # Error on wrong printf() arguments
$<$<COMPILE_LANGUAGE:CXX>:${flag_override_}> # Enforce the use of the override keyword
#
CACHE STRING "(User editable) Private compiler flags for all configurations.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO -g -O3 CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE -O3 CACHE STRING "(User editable) Private compiler flags for Release configuration.")

66
cython/gtsam/utils/plot.py
View File

@ -135,7 +135,8 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
axes.add_patch(e1)
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 2D pose on given figure with given `axis_length`.
@ -144,6 +145,7 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
pose (gtsam.Pose2): The pose to be plotted.
axis_length (float): The length of the camera axes.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
"""
# get figure object
fig = plt.figure(fignum)
@ -151,6 +153,12 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
plot_pose2_on_axes(axes, pose, axis_length=axis_length,
covariance=covariance)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig
def plot_point3_on_axes(axes, point, linespec, P=None):
"""
@ -167,7 +175,8 @@ def plot_point3_on_axes(axes, point, linespec, P=None):
plot_covariance_ellipse_3d(axes, point.vector(), P)
def plot_point3(fignum, point, linespec, P=None):
def plot_point3(fignum, point, linespec, P=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 3D point on given figure with given `linespec`.
@ -176,13 +185,25 @@ def plot_point3(fignum, point, linespec, P=None):
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
Returns:
fig: The matplotlib figure.
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_point3_on_axes(axes, point, linespec, P)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
def plot_3d_points(fignum, values, linespec="g*", marginals=None):
return fig
def plot_3d_points(fignum, values, linespec="g*", marginals=None,
title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plots the Point3s in `values`, with optional covariances.
Finds all the Point3 objects in the given Values object and plots them.
@ -193,7 +214,9 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dictionary consisting of points to be plotted.
linespec (string): String representing formatting options for Matplotlib.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
keys = values.keys()
@ -208,12 +231,16 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
else:
covariance = None
plot_point3(fignum, point, linespec, covariance)
fig = plot_point3(fignum, point, linespec, covariance,
axis_labels=axis_labels)
except RuntimeError:
continue
# I guess it's not a Point3
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
"""
@ -251,7 +278,8 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
plot_covariance_ellipse_3d(axes, origin, gPp)
def plot_pose3(fignum, pose, axis_length=0.1, P=None):
def plot_pose3(fignum, pose, axis_length=0.1, P=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 3D pose on given figure with given `axis_length`.
@ -260,6 +288,10 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None):
pose (gtsam.Pose3): 3D pose to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
Returns:
fig: The matplotlib figure.
"""
# get figure object
fig = plt.figure(fignum)
@ -267,8 +299,15 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None):
plot_pose3_on_axes(axes, pose, P=P,
axis_length=axis_length)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
def plot_trajectory(fignum, values, scale=1, marginals=None):
return fig
def plot_trajectory(fignum, values, scale=1, marginals=None,
title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a complete 3D trajectory using poses in `values`.
@ -278,6 +317,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
scale (float): Value to scale the poses by.
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
Used to plot uncertainty bounds.
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
pose3Values = gtsam.utilities_allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
@ -303,8 +344,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
else:
covariance = None
plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale)
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
lastIndex = i
@ -318,8 +359,11 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
else:
covariance = None
plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale)
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
except:
pass
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())

4
examples/CameraResectioning.cpp
View File

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

16
examples/Data/example_with_vertices.g2o Normal file
View File

@ -0,0 +1,16 @@
VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
VERTEX_TRACKXYZ 7782220156096217088 10 10 10
VERTEX_TRACKXYZ 7782220156096217089 -10 10 10
VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10
VERTEX_TRACKXYZ 7782220156096217091 10 -10 10
VERTEX_TRACKXYZ 7782220156096217092 10 10 -10
VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10
VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10
VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10

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
// must also calculate the Jacobians for this measurement function, if requested.
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:
// error_x = pose.x - measurement.x
// 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
// circumstances, the following code that employs the default copy constructor should
// work fine.
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }

6
examples/SolverComparer.cpp
View File

@ -50,11 +50,11 @@
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>
#include <fstream>
#include <iostream>
#include <random>
#ifdef GTSAM_USE_TBB
#include <tbb/task_arena.h> // tbb::task_arena
@ -554,8 +554,8 @@ void runCompare()
void runPerturb()
{
// Set up random number generator
boost::mt19937 rng;
boost::normal_distribution<double> normal(0.0, perturbationNoise);
std::mt19937 rng;
std::normal_distribution<double> normal(0.0, perturbationNoise);
// Perturb values
VectorValues noise;

24
gtsam/base/GenericValue.h
View File

@ -70,7 +70,7 @@ public:
}
/// equals implementing generic Value interface
virtual bool equals_(const Value& p, double tol = 1e-9) const {
bool equals_(const Value& p, double tol = 1e-9) const override {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
// Return the result of using the equals traits for the derived class
@ -91,7 +91,7 @@ public:
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
*/
virtual Value* clone_() const {
Value* clone_() const override {
GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
return ptr;
}
@ -99,19 +99,19 @@ public:
/**
* Destroy and deallocate this object, only if it was originally allocated using clone_().
*/
virtual void deallocate_() const {
void deallocate_() const override {
delete this;
}
/**
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
virtual boost::shared_ptr<Value> clone() const {
boost::shared_ptr<Value> clone() const override {
return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
}
/// Generic Value interface version of retract
virtual Value* retract_(const Vector& delta) const {
Value* retract_(const Vector& delta) const override {
// Call retract on the derived class using the retract trait function
const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
@ -122,7 +122,7 @@ public:
}
/// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const {
Vector localCoordinates_(const Value& value2) const override {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2);
@ -142,12 +142,12 @@ public:
}
/// Return run-time dimensionality
virtual size_t dim() const {
size_t dim() const override {
return traits<T>::GetDimension(value_);
}
/// Assignment operator
virtual Value& operator=(const Value& rhs) {
Value& operator=(const Value& rhs) override {
// Cast the base class Value pointer to a derived class pointer
const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);
@ -198,4 +198,12 @@ const ValueType& Value::cast() const {
return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
}
/** Functional constructor of GenericValue<T> so T can be automatically deduced
*/
template<class T>
GenericValue<T> genericValue(const T& v) {
return GenericValue<T>(v);
}
} /* namespace gtsam */

10
gtsam/base/ThreadsafeException.h
View File

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

2
gtsam/base/timing.cpp
View File

@ -89,7 +89,7 @@ void TimingOutline::print(const std::string& outline) const {
childOrder[child.second->myOrder_] = child.second;
}
// Print children
for(const ChildOrder::value_type order_child: childOrder) {
for(const ChildOrder::value_type& order_child: childOrder) {
std::string childOutline(outline);
childOutline += "| ";
order_child.second->print(childOutline);

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

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

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

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

2
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -69,7 +69,7 @@ namespace gtsam {
for(Key j: f.keys()) cs[j] = f.cardinality(j);
// Convert map into keys
DiscreteKeys keys;
for(const DiscreteKey& key: cs)
for(const std::pair<const Key,size_t>& key: cs)
keys.push_back(key);
// apply operand
ADT result = ADT::apply(f, op);

12
gtsam/discrete/DecisionTreeFactor.h
View File

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

1
gtsam/discrete/DiscreteBayesTree.h
View File

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

6
gtsam/discrete/DiscreteConditional.h
View File

@ -85,10 +85,10 @@ public:
/// GTSAM-style print
void print(const std::string& s = "Discrete Conditional: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// GTSAM-style equals
bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
/// @}
/// @name Standard Interface
@ -102,7 +102,7 @@ public:
}
/// Evaluate, just look up in AlgebraicDecisonTree
virtual double operator()(const Values& values) const {
double operator()(const Values& values) const override {
return Potentials::operator()(values);
}

2
gtsam/discrete/Potentials.cpp
View File

@ -56,7 +56,7 @@ bool Potentials::equals(const Potentials& other, double tol) const {
/* ************************************************************************* */
void Potentials::print(const string& s, const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: {";
for (const DiscreteKey& key : cardinalities_)
for (const std::pair<const Key,size_t>& key : cardinalities_)
cout << formatter(key.first) << ":" << key.second << ", ";
cout << "}" << endl;
ADT::print(" ");

4
gtsam/geometry/Cal3DS2.h
View File

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

2
gtsam/geometry/Cal3Unified.h
View File

@ -75,7 +75,7 @@ public:
/// @{
/// print with optional string
virtual void print(const std::string& s = "") const ;
void print(const std::string& s = "") const override;
/// assert equality up to a tolerance
bool equals(const Cal3Unified& K, double tol = 10e-9) const;

2
gtsam/geometry/PinholeCamera.h
View File

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

2
gtsam/geometry/PinholePose.h
View File

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

2
gtsam/geometry/PinholeSet.h
View File

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

4
gtsam/geometry/SO4.cpp
View File

@ -35,7 +35,7 @@ namespace gtsam {
// TODO(frank): is this better than SOn::Random?
// /* *************************************************************************
// */ static Vector3 randomOmega(boost::mt19937 &rng) {
// */ static Vector3 randomOmega(std::mt19937 &rng) {
// static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
// return Unit3::Random(rng).unitVector() * randomAngle(rng);
// }
@ -43,7 +43,7 @@ namespace gtsam {
// /* *************************************************************************
// */
// // Create random SO(4) element using direct product of lie algebras.
// SO4 SO4::Random(boost::mt19937 &rng) {
// SO4 SO4::Random(std::mt19937 &rng) {
// Vector6 delta;
// delta << randomOmega(rng), randomOmega(rng);
// return SO4::Expmap(delta);

2
gtsam/geometry/SO4.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
using SO4 = SO<4>;
// /// Random SO(4) element (no big claims about uniformity)
// static SO4 Random(boost::mt19937 &rng);
// static SO4 Random(std::mt19937 &rng);
// Below are all declarations of SO<4> specializations.
// They are *defined* in SO4.cpp.

2
gtsam/inference/BayesTree.h
View File

@ -299,7 +299,7 @@ namespace gtsam {
this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
}
void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const {
void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const override {
clique->print(s + "stored clique", formatter);
}
};

2
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -100,7 +100,7 @@ namespace gtsam {
bool equals(const DERIVED& other, double tol = 1e-9) const;
/** print this node */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface

6
gtsam/inference/inferenceExceptions.h
View File

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

2
gtsam/linear/BinaryJacobianFactor.h
View File

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

2
gtsam/linear/ConjugateGradientSolver.h
View File

@ -80,7 +80,7 @@ public:
void print() const { Base::print(); }
virtual void print(std::ostream &os) const;
void print(std::ostream &os) const override;
static std::string blasTranslator(const BLASKernel k) ;
static BLASKernel blasTranslator(const std::string &s) ;

1
gtsam/linear/GaussianBayesTree.h
View File

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

4
gtsam/linear/GaussianConditional.h
View File

@ -88,10 +88,10 @@ namespace gtsam {
/** print */
void print(const std::string& = "GaussianConditional",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/** equals function */
bool equals(const GaussianFactor&cg, double tol = 1e-9) const;
bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }

2
gtsam/linear/GaussianDensity.h
View File

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

5
gtsam/linear/GaussianISAM.h
View File

@ -20,6 +20,7 @@
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/base/Testable.h>
namespace gtsam {
@ -43,4 +44,8 @@ namespace gtsam {
};
/// traits
template <>
struct traits<GaussianISAM> : public Testable<GaussianISAM> {};
}

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() {}
double weight(double /*error*/) const { return 1.0; }
double loss(double distance) const { return 0.5 * distance * distance; }
void print(const std::string &s) const;
bool equals(const Base & /*expected*/, double /*tol*/) const { return true; }
double weight(double /*error*/) const override { return 1.0; }
double loss(double distance) const override { return 0.5 * distance * distance; }
void print(const std::string &s) const override;
bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
static shared_ptr Create();
private:

6
gtsam/linear/PCGSolver.h
View File

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

2
gtsam/linear/Preconditioner.cpp
View File

@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build(
/* getting the block diagonals over the factors */
std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
for ( const Matrix hessian: hessianMap | boost::adaptors::map_values)
for (const Matrix& hessian: hessianMap | boost::adaptors::map_values)
blocks.push_back(hessian);
/* if necessary, allocating the memory for cacheing the factorization results */

16
gtsam/linear/Preconditioner.h
View File

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

8
gtsam/linear/RegularHessianFactor.h
View File

@ -109,8 +109,8 @@ private:
public:
/** y += alpha * A'*A*x */
virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const {
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const override {
HessianFactor::multiplyHessianAdd(alpha, x, y);
}
@ -182,7 +182,7 @@ public:
}
/** Return the diagonal of the Hessian for this factor (raw memory version) */
virtual void hessianDiagonal(double* d) const {
void hessianDiagonal(double* d) const override {
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
@ -193,7 +193,7 @@ public:
}
/// Add gradient at zero to d TODO: is it really the goal to add ??
virtual void gradientAtZero(double* d) const {
void gradientAtZero(double* d) const override {
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {

10
gtsam/linear/RegularJacobianFactor.h
View File

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

2
gtsam/linear/SubgraphSolver.h
View File

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

2
gtsam/linear/VectorValues.cpp
View File

@ -45,7 +45,7 @@ namespace gtsam {
/* ************************************************************************* */
VectorValues::VectorValues(const Vector& x, const Dims& dims) {
typedef pair<Key, size_t> Pair;
using Pair = pair<const Key, size_t>;
size_t j = 0;
for (const Pair& v : dims) {
Key key;

6
gtsam/linear/linearExceptions.cpp
View File

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

14
gtsam/linear/linearExceptions.h
View File

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

8
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -137,7 +137,7 @@ TEST( GaussianBayesNet, optimize3 )
}
/* ************************************************************************* */
TEST(GaussianBayesNet, ordering)
TEST(GaussianBayesNet, ordering)
{
Ordering expected;
expected += _x_, _y_;
@ -155,7 +155,7 @@ TEST( GaussianBayesNet, MatrixStress )
bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
const VectorValues expected = bn.optimize();
for (const auto keys :
for (const auto& keys :
{KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
@ -183,7 +183,7 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
EXPECT(assert_equal(expected, actual));
const auto ordering = noisyBayesNet.ordering();
const Matrix R = smallBayesNet.matrix(ordering).first;
const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
@ -206,7 +206,7 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
EXPECT(assert_equal(expected, actual));
const auto ordering = noisyBayesNet.ordering();
const Matrix R = noisyBayesNet.matrix(ordering).first;
const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);

10
gtsam/navigation/AHRSFactor.h
View File

@ -158,14 +158,14 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
gtsam::NonlinearFactor::shared_ptr clone() const override;
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const;
bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
/// Access the preintegrated measurements.
const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
@ -178,7 +178,7 @@ public:
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const;
boost::none) const override;
/// predicted states from IMU
/// 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
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/** vector of errors */
virtual Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const {
Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const override {
return attitudeError(nRb, H);
}
@ -182,21 +182,21 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/** vector of errors */
virtual Vector evaluateError(const Pose3& nTb, //
boost::optional<Matrix&> H = boost::none) const {
Vector evaluateError(const Pose3& nTb, //
boost::optional<Matrix&> H = boost::none) const override {
Vector e = attitudeError(nTb.rotation(), H);
if (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)));
}
void print(const std::string& s="") const;
bool equals(const PreintegratedRotationParams& other, double tol) const;
void print(const std::string& s="") const override;
bool equals(const PreintegratedRotationParams& other, double tol) const override;
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
@ -305,7 +305,7 @@ public:
virtual ~CombinedImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
gtsam::NonlinearFactor::shared_ptr clone() const override;
/** implement functions needed for Testable */
@ -314,11 +314,11 @@ public:
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const CombinedImuFactor&);
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// @}
/** Access the preintegrated measurements. */
@ -336,7 +336,7 @@ public:
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none, boost::optional<Matrix&> H3 = boost::none,
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
/// @deprecated typename

20
gtsam/navigation/GPSFactor.h
View File

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

20
gtsam/navigation/ImuFactor.h
View File

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

16
gtsam/navigation/MagFactor.h
View File

@ -53,7 +53,7 @@ public:
}
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor(*this)));
}
@ -73,7 +73,7 @@ public:
* @brief vector of errors
*/
Vector evaluateError(const Rot2& nRb,
boost::optional<Matrix&> H = boost::none) const {
boost::optional<Matrix&> H = boost::none) const override {
// measured bM = nRb<52> * nM + b
Point3 hx = unrotate(nRb, nM_, H) + bias_;
return (hx - measured_);
@ -102,7 +102,7 @@ public:
}
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor1(*this)));
}
@ -111,7 +111,7 @@ public:
* @brief vector of errors
*/
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const {
boost::optional<Matrix&> H = boost::none) const override {
// measured bM = nRb<52> * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_);
@ -138,7 +138,7 @@ public:
}
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor2(*this)));
}
@ -150,7 +150,7 @@ public:
*/
Vector evaluateError(const Point3& nM, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
boost::none) const override {
// measured bM = nRb<52> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H2)
@ -179,7 +179,7 @@ public:
}
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor3(*this)));
}
@ -192,7 +192,7 @@ public:
Vector evaluateError(const double& scale, const Unit3& direction,
const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const {
boost::none) const override {
// measured bM = nRb<52> * nM + b, where b is unknown bias
Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
Point3 hx = scale * rotated.point3() + bias;

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)));
}
void print(const std::string& s="") const;
bool equals(const PreintegratedRotationParams& other, double tol) const;
void print(const std::string& s="") const override;
bool equals(const PreintegratedRotationParams& other, double tol) const override;
void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }

4
gtsam/navigation/TangentPreintegration.cpp
View File

@ -68,7 +68,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const Rot3 R(local.expmap());
const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
@ -110,7 +110,7 @@ void TangentPreintegration::update(const Vector3& measuredAcc,
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
// Possibly correct for sensor pose by converting to body frame
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,

23
gtsam/nonlinear/ExpressionFactor.h
View File

@ -27,8 +27,13 @@
namespace gtsam {
/**
* Factor that supports arbitrary expressions via AD
* Factor that supports arbitrary expressions via AD.
*
* Arbitrary instances of this template can be directly inserted into a factor
* graph for optimization. However, to enable the correct (de)serialization of
* such instances, the user should declare derived classes from this template,
* implementing expresion(), serialize(), clone(), print(), and defining the
* corresponding `struct traits<NewFactor> : public Testable<NewFactor> {}`.
*/
template<typename T>
class ExpressionFactor: public NoiseModelFactor {
@ -68,13 +73,13 @@ protected:
/// print relies on Testable traits being defined for T
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
NoiseModelFactor::print(s, keyFormatter);
traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
}
/// equals relies on Testable traits being defined for T
bool equals(const NonlinearFactor& f, double tol) const {
bool equals(const NonlinearFactor& f, double tol) const override {
const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
return p && NoiseModelFactor::equals(f, tol) &&
traits<T>::Equals(measured_, p->measured_, tol) &&
@ -86,8 +91,8 @@ protected:
* We override this method to provide
* both the function evaluation and its derivative(s) in H.
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
// NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
@ -99,7 +104,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
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
@ -138,7 +143,7 @@ protected:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
@ -263,7 +268,7 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
private:
/// 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]);
}

9
gtsam/nonlinear/FunctorizedFactor.h
View File

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

3
gtsam/nonlinear/ISAM2Clique.h
View File

@ -51,6 +51,7 @@ class GTSAM_EXPORT ISAM2Clique
/// Default constructor
ISAM2Clique() : Base() {}
virtual ~ISAM2Clique() = default;
/// Copy constructor, does *not* copy solution pointers as these are invalid
/// in different trees.
@ -85,7 +86,7 @@ class GTSAM_EXPORT ISAM2Clique
/** print this node */
void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
void optimizeWildfire(const KeySet& replaced, double threshold,
KeySet* changed, VectorValues* delta,

12
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -59,10 +59,10 @@ public:
// Testable
/** print */
GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
/** Check if two factors are equal */
GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
// NonlinearFactor
@ -74,10 +74,10 @@ public:
*
* @return nonlinear error if linearizationPoint present, zero otherwise
*/
GTSAM_EXPORT double error(const Values& c) const;
GTSAM_EXPORT double error(const Values& c) const override;
/** get the dimension of the factor: rows of linear factor */
GTSAM_EXPORT size_t dim() const;
GTSAM_EXPORT size_t dim() const override;
/** Extract the linearization point used in recalculating error */
const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; }
@ -98,7 +98,7 @@ public:
* TODO: better approximation of relinearization
* TODO: switchable modes for approximation technique
*/
GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const;
GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const override;
/**
* Creates an anti-factor directly
@ -116,7 +116,7 @@ public:
*
* Clones the underlying linear factor
*/
NonlinearFactor::shared_ptr clone() const {
NonlinearFactor::shared_ptr clone() const override {
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
}

26
gtsam/nonlinear/NonlinearEquality.h
View File

@ -107,15 +107,15 @@ public:
/// @name Testable
/// @{
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
traits<VALUE>::Print(feasible_, "Feasible Point:\n");
std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_) << std::endl;
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&f);
return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
&& std::abs(error_gain_ - e->error_gain_) < tol;
@ -126,7 +126,7 @@ public:
/// @{
/** actual error function calculation */
virtual double error(const Values& c) const {
double error(const Values& c) const override {
const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) {
@ -138,7 +138,7 @@ public:
/** error function */
Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) const {
boost::optional<Matrix&> H = boost::none) const override {
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
@ -158,7 +158,7 @@ public:
}
// Linearize is over-written, because base linearization tries to whiten
virtual GaussianFactor::shared_ptr linearize(const Values& x) const {
GaussianFactor::shared_ptr linearize(const Values& x) const override {
const T& xj = x.at<T>(this->key());
Matrix A;
Vector b = evaluateError(xj, A);
@ -168,7 +168,7 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
@ -242,14 +242,14 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** g(x) with optional derivative */
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const {
boost::optional<Matrix&> H = boost::none) const override {
if (H)
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
@ -257,8 +257,8 @@ public:
}
/** Print */
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
<< ")," << "\n";
this->noiseModel_->print();
@ -317,14 +317,14 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** g(x) with optional derivative2 */
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;
if (H1) *H1 = -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
#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>( \
gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }
#endif
@ -195,14 +195,14 @@ protected:
public:
/** Print */
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const {
size_t dim() const override {
return noiseModel_->dim();
}
@ -242,14 +242,14 @@ public:
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const Values& c) const;
double error(const Values& c) const override;
/**
* Linearize a non-linearFactorN to get a GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const;
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
@ -315,7 +315,7 @@ public:
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class.
*/
virtual Vector unwhitenedError(const Values& x, boost::optional<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)) {
const X& x1 = x.at<X>(keys_[0]);
if(H) {
@ -389,7 +389,7 @@ public:
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<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)) {
const X1& x1 = x.at<X1>(keys_[0]);
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
* 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(H)
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
* 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(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]);
@ -631,7 +631,7 @@ public:
/** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<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(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]);
@ -719,7 +719,7 @@ public:
/** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<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(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]);

15
gtsam/nonlinear/PriorFactor.h
View File

@ -65,15 +65,15 @@ namespace gtsam {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
void print(const std::string& s,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
traits<T>::Print(prior_, " prior mean: ");
if (this->noiseModel_)
@ -83,7 +83,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This* e = dynamic_cast<const This*> (&expected);
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 */
/** 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));
// manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
@ -117,4 +117,9 @@ namespace gtsam {
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
/// traits
template<class VALUE>
struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
} /// namespace gtsam

14
gtsam/nonlinear/Values.cpp
View File

@ -52,6 +52,12 @@ namespace gtsam {
Values::Values(Values&& other) : values_(std::move(other.values_)) {
}
/* ************************************************************************* */
Values::Values(std::initializer_list<ConstKeyValuePair> init) {
for (const auto &kv : init)
insert(kv.key, kv.value);
}
/* ************************************************************************* */
Values::Values(const Values& other, const VectorValues& delta) {
for (const_iterator key_value = other.begin(); key_value != other.end(); ++key_value) {
@ -215,7 +221,7 @@ namespace gtsam {
}
/* ************************************************************************* */
const char* ValuesKeyAlreadyExists::what() const throw() {
const char* ValuesKeyAlreadyExists::what() const noexcept {
if(message_.empty())
message_ =
"Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists.";
@ -223,7 +229,7 @@ namespace gtsam {
}
/* ************************************************************************* */
const char* ValuesKeyDoesNotExist::what() const throw() {
const char* ValuesKeyDoesNotExist::what() const noexcept {
if(message_.empty())
message_ =
"Attempting to " + std::string(operation_) + " the key \"" +
@ -232,7 +238,7 @@ namespace gtsam {
}
/* ************************************************************************* */
const char* ValuesIncorrectType::what() const throw() {
const char* ValuesIncorrectType::what() const noexcept {
if(message_.empty()) {
std::string storedTypeName = demangle(storedTypeId_.name());
std::string requestedTypeName = demangle(requestedTypeId_.name());
@ -252,7 +258,7 @@ namespace gtsam {
}
/* ************************************************************************* */
const char* NoMatchFoundForFixed::what() const throw() {
const char* NoMatchFoundForFixed::what() const noexcept {
if(message_.empty()) {
ostringstream oss;
oss

43
gtsam/nonlinear/Values.h
View File

@ -149,6 +149,13 @@ namespace gtsam {
/** Move constructor */
Values(Values&& other);
/** Constructor from initializer list. Example usage:
* \code
* Values v = {{k1, genericValue(pose1)}, {k2, genericValue(point2)}};
* \endcode
*/
Values(std::initializer_list<ConstKeyValuePair> init);
/** Construct from a Values and an update vector: identical to other.retract(delta) */
Values(const Values& other, const VectorValues& delta);
@ -432,16 +439,16 @@ namespace gtsam {
public:
/// Construct with the key-value pair attempted to be added
ValuesKeyAlreadyExists(Key key) throw() :
ValuesKeyAlreadyExists(Key key) noexcept :
key_(key) {}
virtual ~ValuesKeyAlreadyExists() throw() {}
virtual ~ValuesKeyAlreadyExists() noexcept {}
/// The duplicate key that was attempted to be added
Key key() const throw() { return key_; }
Key key() const noexcept { return key_; }
/// The message to be displayed to the user
GTSAM_EXPORT virtual const char* what() const throw();
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */
@ -455,16 +462,16 @@ namespace gtsam {
public:
/// Construct with the key that does not exist in the values
ValuesKeyDoesNotExist(const char* operation, Key key) throw() :
ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
operation_(operation), key_(key) {}
virtual ~ValuesKeyDoesNotExist() throw() {}
virtual ~ValuesKeyDoesNotExist() noexcept {}
/// The key that was attempted to be accessed that does not exist
Key key() const throw() { return key_; }
Key key() const noexcept { return key_; }
/// The message to be displayed to the user
GTSAM_EXPORT virtual const char* what() const throw();
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */
@ -480,13 +487,13 @@ namespace gtsam {
public:
/// Construct with the key that does not exist in the values
ValuesIncorrectType(Key key,
const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
virtual ~ValuesIncorrectType() throw() {}
virtual ~ValuesIncorrectType() noexcept {}
/// The key that was attempted to be accessed that does not exist
Key key() const throw() { return key_; }
Key key() const noexcept { return key_; }
/// The typeid of the value stores in the Values
const std::type_info& storedTypeId() const { return storedTypeId_; }
@ -495,18 +502,18 @@ namespace gtsam {
const std::type_info& requestedTypeId() const { return requestedTypeId_; }
/// The message to be displayed to the user
GTSAM_EXPORT virtual const char* what() const throw();
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */
class DynamicValuesMismatched : public std::exception {
public:
DynamicValuesMismatched() throw() {}
DynamicValuesMismatched() noexcept {}
virtual ~DynamicValuesMismatched() throw() {}
virtual ~DynamicValuesMismatched() noexcept {}
virtual const char* what() const throw() {
const char* what() const noexcept override {
return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
}
};
@ -522,14 +529,14 @@ namespace gtsam {
mutable std::string message_;
public:
NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () :
NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
}
virtual ~NoMatchFoundForFixed() throw () {
virtual ~NoMatchFoundForFixed() noexcept {
}
GTSAM_EXPORT virtual const char* what() const throw ();
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */

10
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -109,7 +109,7 @@ namespace gtsam {
/// Print
void print(const std::string& p = "WhiteNoiseFactor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(p, keyFormatter);
std::cout << p + ".z: " << z_ << std::endl;
}
@ -119,12 +119,12 @@ namespace gtsam {
/// @{
/// get the dimension of the factor (number of rows on linearization)
virtual size_t dim() const {
size_t dim() const override {
return 2;
}
/// Calculate the error of the factor, typically equal to log-likelihood
inline double error(const Values& x) const {
double error(const Values& x) const override {
return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
}
@ -153,7 +153,7 @@ namespace gtsam {
/// @{
/// 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 p = x.at<double>(precisionKey_);
Key j1 = meanKey_;
@ -163,7 +163,7 @@ namespace gtsam {
// TODO: Frank commented this out for now, can it go?
// /// @return a deep copy of this factor
// virtual gtsam::NonlinearFactor::shared_ptr clone() const {
// gtsam::NonlinearFactor::shared_ptr clone() const override {
// return boost::static_pointer_cast<gtsam::NonlinearFactor>(
// 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);
}
virtual void _print(const std::string& indent) const {
void _print(const std::string& indent) const override {
derived().print(indent);
}
// Called from base class non-virtual inline method startReverseAD2
// Calls non-virtual function startReverseAD4, implemented in Derived (ExpressionNode::Record)
virtual void _startReverseAD3(JacobianMap& jacobians) const {
void _startReverseAD3(JacobianMap& jacobians) const override {
derived().startReverseAD4(jacobians);
}
virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const {
void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(
void _reverseAD3(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const {
JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
JacobianMap& jacobians) const {
void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
JacobianMap& jacobians) const {
void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
JacobianMap& jacobians) const {
void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
JacobianMap& jacobians) const {
void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
JacobianMap& jacobians) const {
void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
JacobianMap& jacobians) const override {
derived().reverseAD4(dFdT, jacobians);
}
};

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

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

6
gtsam/nonlinear/nonlinearExceptions.h
View File

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

8
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -168,7 +168,11 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
Vector9 P = Camera().localCoordinates(camera);
Vector3 X = point;
#ifdef GTSAM_POSE3_EXPMAP
Vector2 expectedMeasurement(1.3124675, 1.2057287);
#else
Vector2 expectedMeasurement(1.2431567, 1.2525694);
#endif
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
}
@ -177,7 +181,11 @@ Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
// Check that Local worked as expected
TEST(AdaptAutoDiff, Local) {
using namespace example;
#ifdef GTSAM_POSE3_EXPMAP
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished();
#else
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
#endif
EXPECT(equal_with_abs_tol(expectedP, P));
Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely!
EXPECT(equal_with_abs_tol(expectedX, X));

3
gtsam/nonlinear/tests/testCallRecord.cpp
View File

@ -98,7 +98,8 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
friend struct internal::CallRecordImplementor;
};
internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(nullptr);
internal::JacobianMap* NJM_ptr = static_cast<internal::JacobianMap *>(nullptr);
internal::JacobianMap & NJM = *NJM_ptr;
/* ************************************************************************* */
typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;

2
gtsam/nonlinear/tests/testFunctorizedFactor.cpp
View File

@ -131,7 +131,7 @@ TEST(FunctorizedFactor, Print) {
"FunctorizedFactor(X0)\n"
" measurement: [\n"
" 1, 0;\n"
" 0, 1\n"
" 0, 1\n"
"]\n"
" noise model sigmas: 1 1 1 1 1 1 1 1 1\n";

30
gtsam/nonlinear/tests/testValues.cpp
View File

@ -606,6 +606,36 @@ TEST(Values, Demangle) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Values, brace_initializer) {
const Pose2 poseA(1.0, 2.0, 0.3), poseC(.0, .0, .0);
const Pose3 poseB(Pose2(0.1, 0.2, 0.3));
{
Values values;
EXPECT_LONGS_EQUAL(0, values.size());
values = { {key1, genericValue(1.0)} };
EXPECT_LONGS_EQUAL(1, values.size());
CHECK(values.at<double>(key1) == 1.0);
}
{
Values values = { {key1, genericValue(poseA)}, {key2, genericValue(poseB)} };
EXPECT_LONGS_EQUAL(2, values.size());
EXPECT(assert_equal(values.at<Pose2>(key1), poseA));
EXPECT(assert_equal(values.at<Pose3>(key2), poseB));
}
// Test exception: duplicated key:
{
Values values;
CHECK_EXCEPTION((values = {
{key1, genericValue(poseA)},
{key2, genericValue(poseB)},
{key1, genericValue(poseC)}
}), std::exception);
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

4
gtsam/sam/BearingFactor.h
View File

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

8
gtsam/sam/BearingRangeFactor.h
View File

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

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

14
gtsam/slam/AntiFactor.h
View File

@ -52,20 +52,20 @@ namespace gtsam {
virtual ~AntiFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "AntiFactor version of:" << std::endl;
factor_->print(s, keyFormatter);
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
}
@ -77,16 +77,16 @@ namespace gtsam {
* For the AntiFactor, this will have the same magnitude of the original factor,
* but the opposite sign.
*/
double error(const Values& c) const { return -factor_->error(c); }
double error(const Values& c) const override { return -factor_->error(c); }
/** get the dimension of the factor (same as the original factor) */
size_t dim() const { return factor_->dim(); }
size_t dim() const override { return factor_->dim(); }
/**
* Checks whether this factor should be used based on a set of values.
* The AntiFactor will have the same 'active' profile as the original factor.
*/
bool active(const Values& c) const { return factor_->active(c); }
bool active(const Values& c) const override { return factor_->active(c); }
/**
* Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor
@ -94,7 +94,7 @@ namespace gtsam {
* returns a Jacobian instead of a full Hessian), but with the opposite sign. This
* effectively cancels the effect of the original factor on the factor graph.
*/
boost::shared_ptr<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
GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c);

10
gtsam/slam/BetweenFactor.h
View File

@ -63,14 +63,14 @@ namespace gtsam {
virtual ~BetweenFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
@ -79,7 +79,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected);
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 */
/** vector of errors */
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
#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;
/** active when constraint *NOT* met */
bool active(const Values& c) const {
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X>(this->key()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const {
boost::none) const override {
Matrix D;
double error = value(x, D) - threshold_;
if (H) {
@ -126,7 +126,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
boost::optional<Matrix&> H2 = boost::none) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const {
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@ -134,7 +134,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
boost::optional<Matrix&> H2 = boost::none) const override {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
if (H1) {

12
gtsam/slam/EssentialMatrixConstraint.h
View File

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

24
gtsam/slam/EssentialMatrixFactor.h
View File

@ -58,14 +58,14 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor with measurements\n ("
<< vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
@ -74,7 +74,7 @@ public:
/// vector of errors returns 1D vector
Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
boost::none) const {
boost::none) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
@ -131,14 +131,14 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.transpose() << ")' and (" << pn_.transpose()
@ -152,7 +152,7 @@ public:
*/
Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const {
boost::none) const override {
// We have point x,y in image 1
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
@ -250,14 +250,14 @@ public:
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
}
@ -269,7 +269,7 @@ public:
*/
Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const {
boost::none) const override {
if (!DE) {
// Convert E from body to camera frame
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.
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.
}
};
@ -78,7 +78,7 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
/// Error is just Frobenius norm between rotation matrices.
Vector evaluateError(const Rot& R1, const Rot& R2,
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);
if (H1) *H1 = -*H1;
return error;
@ -110,7 +110,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
/// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2,
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_);
Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
@ -139,7 +139,7 @@ class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn>
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector evaluateError(const SOn& Q1, const SOn& Q2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
boost::optional<Matrix&> H2 = boost::none) const override;
};
} // namespace gtsam

18
gtsam/slam/GeneralSFMFactor.h
View File

@ -96,7 +96,7 @@ public:
virtual ~GeneralSFMFactor() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
@ -105,7 +105,7 @@ public:
* @param s optional string naming the factor
* @param keyFormatter optional formatter for printing out Symbols
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
traits<Point2>::Print(measured_, s + ".z");
}
@ -113,14 +113,14 @@ public:
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/** h(x)-z */
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 {
return camera.project2(point,H1,H2) - measured_;
}
@ -133,7 +133,7 @@ public:
}
/// 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
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
@ -230,7 +230,7 @@ public:
virtual ~GeneralSFMFactor2() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
@ -239,7 +239,7 @@ public:
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
traits<Point2>::Print(measured_, s + ".z");
}
@ -247,7 +247,7 @@ public:
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&p);
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,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const
boost::optional<Matrix&> H3=boost::none) const override
{
try {
Camera camera(pose3,calib);

18
gtsam/slam/OrientedPlane3Factor.h
View File

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

8
gtsam/slam/PoseRotationPrior.h
View File

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

8
gtsam/slam/PoseTranslationPrior.h
View File

@ -54,12 +54,12 @@ public:
const Translation& measured() const { return measured_; }
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** 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 Rotation& R = pose.rotation();
const int tDim = traits<Translation>::GetDimension(newTrans);
@ -74,13 +74,13 @@ public:
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s + "PoseTranslationPrior", keyFormatter);
traits<Translation>::Print(measured_, "Measured Translation");
}

8
gtsam/slam/ProjectionFactor.h
View File

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

10
gtsam/slam/ReferenceFrameFactor.h
View File

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

16
gtsam/slam/RotateFactor.h
View File

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

6
gtsam/slam/SmartFactorBase.h
View File

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

10
gtsam/slam/SmartProjectionFactor.h
View File

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

8
gtsam/slam/SmartProjectionPoseFactor.h
View File

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

8
gtsam/slam/StereoFactor.h
View File

@ -91,7 +91,7 @@ public:
virtual ~GenericStereoFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
@ -100,7 +100,7 @@ public:
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
if(this->body_P_sensor_)
@ -110,7 +110,7 @@ public:
/**
* equals
*/
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
return e
&& Base::equals(f)
@ -120,7 +120,7 @@ public:
/** h(x)-z */
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 {
if(body_P_sensor_) {
if(H1) {

10
gtsam/slam/TriangulationFactor.h
View File

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

111
gtsam/slam/dataset.cpp
View File

@ -11,7 +11,10 @@
/**
* @file dataset.cpp
* @date Jan 22, 2010
* @author Kai Ni, Luca Carlone, Frank Dellaert
* @author Kai Ni
* @author Luca Carlone
* @author Frank Dellaert
* @author Varun Agrawal
* @brief utility functions for loading datasets
*/
@ -70,8 +73,8 @@ string findExampleDataFile(const string& name) {
namesToSearch.push_back(name + ".xml");
// Find first name that exists
for(const fs::path& root: rootsToSearch) {
for(const fs::path& name: namesToSearch) {
for(const fs::path root: rootsToSearch) {
for(const fs::path name: namesToSearch) {
if (fs::is_regular_file(root / name))
return (root / name).string();
}
@ -192,8 +195,15 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
}
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/* ************************************************************************* */
boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
return parseVertexPose(is, tag);
}
#endif
/* ************************************************************************* */
boost::optional<IndexedPose> parseVertexPose(istream& is, const string& tag) {
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
Key id;
double x, y, yaw;
@ -204,6 +214,18 @@ boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
}
}
/* ************************************************************************* */
boost::optional<IndexedLandmark> parseVertexLandmark(istream& is, const string& tag) {
if (tag == "VERTEX_XY") {
Key id;
double x, y;
is >> id >> x >> y;
return IndexedLandmark(id, Point2(x, y));
} else {
return boost::none;
}
}
/* ************************************************************************* */
boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
@ -232,12 +254,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
string tag;
// load the poses
// load the poses and landmarks
while (!is.eof()) {
if (!(is >> tag))
break;
const auto indexed_pose = parseVertex(is, tag);
const auto indexed_pose = parseVertexPose(is, tag);
if (indexed_pose) {
Key id = indexed_pose->first;
@ -247,6 +269,16 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
initial->insert(id, indexed_pose->second);
}
const auto indexed_landmark = parseVertexLandmark(is, tag);
if (indexed_landmark) {
Key id = indexed_landmark->first;
// optional filter
if (maxID && id >= maxID)
continue;
initial->insert(id, indexed_landmark->second);
}
is.ignore(LINESIZE, '\n');
}
is.clear(); /* clears the end-of-file and error flags */
@ -429,7 +461,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
const string& filename) {
fstream stream(filename.c_str(), fstream::out);
// save 2D & 3D poses
// save 2D poses
for (const auto& key_value : estimate) {
auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
if (!p) continue;
@ -438,15 +470,34 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
<< pose.y() << " " << pose.theta() << endl;
}
// save 3D poses
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
const Pose3& pose = p->value();
const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
const Pose3& pose = p->value();
const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
}
// save 2D landmarks
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Point2>*>(&key_value.value);
if (!p) continue;
const Point2& point = p->value();
stream << "VERTEX_XY " << key_value.key << " " << point.x() << " "
<< point.y() << endl;
}
// save 3D landmarks
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Point3>*>(&key_value.value);
if (!p) continue;
const Point3& point = p->value();
stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " "
<< point.y() << " " << point.z() << endl;
}
// save edges (2D or 3D)
@ -515,6 +566,7 @@ static Rot3 NormalizedRot3(double w, double x, double y, double z) {
const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
return Rot3::Quaternion(f * w, f * x, f * y, f * z);
}
/* ************************************************************************* */
std::map<Key, Pose3> parse3DPoses(const string& filename) {
ifstream is(filename.c_str());
@ -545,6 +597,30 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
return poses;
}
/* ************************************************************************* */
std::map<Key, Point3> parse3DLandmarks(const string& filename) {
ifstream is(filename.c_str());
if (!is)
throw invalid_argument("parse3DLandmarks: can not find file " + filename);
std::map<Key, Point3> landmarks;
while (!is.eof()) {
char buf[LINESIZE];
is.getline(buf, LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag == "VERTEX_TRACKXYZ") {
Key id;
double x, y, z;
ls >> id >> x >> y >> z;
landmarks.emplace(id, Point3(x, y, z));
}
}
return landmarks;
}
/* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(
const string& filename,
@ -617,11 +693,16 @@ GraphAndValues load3D(const string& filename) {
graph->push_back(factor);
}
const auto poses = parse3DPoses(filename);
Values::shared_ptr initial(new Values);
const auto poses = parse3DPoses(filename);
for (const auto& key_pose : poses) {
initial->insert(key_pose.first, key_pose.second);
}
const auto landmarks = parse3DLandmarks(filename);
for (const auto& key_landmark : landmarks) {
initial->insert(key_landmark.first, key_landmark.second);
}
return make_pair(graph, initial);
}

25
gtsam/slam/dataset.h
View File

@ -76,8 +76,10 @@ enum KernelFunctionType {
/// Return type for auxiliary functions
typedef std::pair<Key, Pose2> IndexedPose;
typedef std::pair<Key, Point2> IndexedLandmark;
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
@ -85,6 +87,24 @@ typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is,
const std::string& tag);
#endif
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
const std::string& tag);
/**
* Parse G2O landmark vertex "id x y"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
const std::string& tag);
/**
* Parse TORO/G2O edge "id1 id2 x y yaw"
@ -162,9 +182,12 @@ using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
/// Parse vertices in 3D TORO graph file into a map of Pose3s.
/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
/// Parse landmarks in 3D g2o graph file into a map of Point3s.
GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const string& filename);
/// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);

38
gtsam/slam/tests/testDataset.cpp
View File

@ -43,13 +43,13 @@ TEST(dataSet, findExampleDataFile) {
}
/* ************************************************************************* */
TEST( dataSet, parseVertex)
TEST( dataSet, parseVertexPose)
{
const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
istringstream is(str);
string tag;
EXPECT(is >> tag);
const auto actual = parseVertex(is, tag);
const auto actual = parseVertexPose(is, tag);
EXPECT(actual);
if (actual) {
EXPECT_LONGS_EQUAL(1, actual->first);
@ -57,6 +57,21 @@ TEST( dataSet, parseVertex)
}
}
/* ************************************************************************* */
TEST( dataSet, parseVertexLandmark)
{
const string str = "VERTEX_XY 1 2.000000 3.000000";
istringstream is(str);
string tag;
EXPECT(is >> tag);
const auto actual = parseVertexLandmark(is, tag);
EXPECT(actual);
if (actual) {
EXPECT_LONGS_EQUAL(1, actual->first);
EXPECT(assert_equal(Point2(2, 3), actual->second));
}
}
/* ************************************************************************* */
TEST( dataSet, parseEdge)
{
@ -182,6 +197,12 @@ TEST(dataSet, readG2o3D) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check landmark parsing
const auto actualLandmarks = parse3DLandmarks(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check graph version
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
@ -252,6 +273,19 @@ TEST(dataSet, readG2oCheckDeterminants) {
const Rot3 R = key_value.second.rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
}
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
EXPECT_LONGS_EQUAL(0, landmarks.size());
}
/* ************************************************************************* */
TEST(dataSet, readG2oLandmarks) {
const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
// Check number of poses and landmarks. Should be 8 each.
const map<Key, Pose3> poses = parse3DPoses(g2oFile);
EXPECT_LONGS_EQUAL(8, poses.size());
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
EXPECT_LONGS_EQUAL(8, landmarks.size());
}
/* ************************************************************************* */

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

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

1
gtsam/symbolic/SymbolicBayesTree.h
View File

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

2
gtsam/symbolic/SymbolicConditional.h
View File

@ -105,7 +105,7 @@ namespace gtsam {
/// @name Testable
/** Print with optional formatter */
void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
virtual void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check equality */
bool equals(const This& c, double tol = 1e-9) const;

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