Merge remote-tracking branch 'origin/develop' into fix/logging_lambda

release/4.3a0
Fan Jiang 2020-07-09 17:07:07 -04:00
commit 686ea137e7
57 changed files with 1794 additions and 143 deletions

View File

@ -14,7 +14,8 @@ addons:
- clang-9 - clang-9
- build-essential pkg-config - build-essential pkg-config
- cmake - cmake
- libpython-dev python-numpy - python3-dev libpython-dev
- python3-numpy
- libboost-all-dev - libboost-all-dev
# before_install: # before_install:

View File

@ -0,0 +1,56 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
FrobeniusFactor unit tests.
Author: Frank Dellaert
"""
# pylint: disable=no-name-in-module, import-error, invalid-name
import unittest
import numpy as np
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
FrobeniusWormholeFactor, SOn)
id = SO4()
v1 = np.array([0, 0, 0, 0.1, 0, 0])
Q1 = SO4.Expmap(v1)
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
Q2 = SO4.Expmap(v2)
class TestFrobeniusFactorSO4(unittest.TestCase):
"""Test FrobeniusFactor factors."""
def test_frobenius_factor(self):
"""Test creation of a factor that calculates the Frobenius norm."""
factor = FrobeniusFactorSO4(1, 2)
actual = factor.evaluateError(Q1, Q2)
expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
np.testing.assert_array_equal(actual, expected)
def test_frobenius_between_factor(self):
"""Test creation of a Frobenius BetweenFactor."""
factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
actual = factor.evaluateError(Q1, Q2)
expected = np.zeros((16,))
np.testing.assert_array_almost_equal(actual, expected)
def test_frobenius_wormhole_factor(self):
"""Test creation of a factor that calculates Shonan error."""
R1 = SO3.Expmap(v1[3:])
R2 = SO3.Expmap(v2[3:])
factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4)
I4 = SOn(4)
Q1 = I4.retract(v1)
Q2 = I4.retract(v2)
actual = factor.evaluateError(Q1, Q2)
expected = np.zeros((12,))
np.testing.assert_array_almost_equal(actual, expected, decimal=4)
if __name__ == "__main__":
unittest.main()

View File

@ -4,6 +4,12 @@ Author: Jing Wu and Frank Dellaert
""" """
# pylint: disable=invalid-name # pylint: disable=invalid-name
import sys
if sys.version_info.major >= 3:
from io import StringIO
else:
from cStringIO import StringIO
import unittest import unittest
from datetime import datetime from datetime import datetime
@ -37,11 +43,19 @@ class TestOptimizeComet(GtsamTestCase):
self.optimizer = gtsam.GaussNewtonOptimizer( self.optimizer = gtsam.GaussNewtonOptimizer(
graph, initial, self.params) graph, initial, self.params)
# setup output capture
self.capturedOutput = StringIO()
sys.stdout = self.capturedOutput
def tearDown(self):
"""Reset print capture."""
sys.stdout = sys.__stdout__
def test_simple_printing(self): def test_simple_printing(self):
"""Test with a simple hook.""" """Test with a simple hook."""
# Provide a hook that just prints # Provide a hook that just prints
def hook(_, error: float): def hook(_, error):
print(error) print(error)
# Only thing we require from optimizer is an iterate method # Only thing we require from optimizer is an iterate method
@ -65,7 +79,7 @@ class TestOptimizeComet(GtsamTestCase):
+ str(time.hour)+":"+str(time.minute)+":"+str(time.second)) + str(time.hour)+":"+str(time.minute)+":"+str(time.second))
# I want to do some comet thing here # I want to do some comet thing here
def hook(optimizer, error: float): def hook(optimizer, error):
comet.log_metric("Karcher error", comet.log_metric("Karcher error",
error, optimizer.iterations()) error, optimizer.iterations())

View File

@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert
""" """
# pylint: disable=invalid-name # pylint: disable=invalid-name
from typing import TypeVar
from gtsam import NonlinearOptimizer, NonlinearOptimizerParams from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
import gtsam import gtsam
T = TypeVar('T')
def optimize(optimizer, check_convergence, hook):
def optimize(optimizer: T, check_convergence, hook):
""" Given an optimizer and a convergence check, iterate until convergence. """ Given an optimizer and a convergence check, iterate until convergence.
After each iteration, hook(optimizer, error) is called. After each iteration, hook(optimizer, error) is called.
After the function, use values and errors to get the result. After the function, use values and errors to get the result.
@ -36,8 +32,8 @@ def optimize(optimizer: T, check_convergence, hook):
current_error = new_error current_error = new_error
def gtsam_optimize(optimizer: NonlinearOptimizer, def gtsam_optimize(optimizer,
params: NonlinearOptimizerParams, params,
hook): hook):
""" Given an optimizer and params, iterate until convergence. """ Given an optimizer and params, iterate until convergence.
After each iteration, hook(optimizer) is called. After each iteration, hook(optimizer) is called.

View File

@ -1,3 +1,3 @@
Cython>=0.25.2 Cython>=0.25.2
backports_abc>=0.5 backports_abc>=0.5
numpy>=1.12.0 numpy>=1.11.0

33
gtsam.h
View File

@ -281,7 +281,7 @@ virtual class Value {
}; };
#include <gtsam/base/GenericValue.h> #include <gtsam/base/GenericValue.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value { virtual class GenericValue : gtsam::Value {
void serializable() const; void serializable() const;
}; };
@ -598,6 +598,7 @@ class SOn {
// Standard Constructors // Standard Constructors
SOn(size_t n); SOn(size_t n);
static gtsam::SOn FromMatrix(Matrix R); static gtsam::SOn FromMatrix(Matrix R);
static gtsam::SOn Lift(size_t n, Matrix R);
// Testable // Testable
void print(string s) const; void print(string s) const;
@ -2835,6 +2836,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys); KarcherMeanFactor(const gtsam::KeyVector& keys);
}; };
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
gtsam::noiseModel::Base* model, size_t d);
template<T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2);
FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2);
};
template<T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2);
};
virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p);
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p, gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
};
//************************************************************************* //*************************************************************************
// Navigation // Navigation
//************************************************************************* //*************************************************************************
@ -2955,6 +2984,7 @@ class PreintegratedImuMeasurements {
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; Vector deltaPij() const;
Vector deltaVij() const; Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const; Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i, gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;
@ -3016,6 +3046,7 @@ class PreintegratedCombinedMeasurements {
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; Vector deltaPij() const;
Vector deltaVij() const; Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const; Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i, gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;

View File

@ -17,7 +17,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
endforeach(eigen_dir) endforeach(eigen_dir)
if(GTSAM_WITH_EIGEN_UNSUPPORTED) if(GTSAM_WITH_EIGEN_UNSUPPORTED)
message("-- Installing Eigen Unsupported modules") message(STATUS "Installing Eigen Unsupported modules")
# do the same for the unsupported eigen folder # do the same for the unsupported eigen folder
file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")

View File

@ -181,7 +181,7 @@ public:
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; };
/// use this macro instead of BOOST_CLASS_EXPORT for GenericValues /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues

View File

@ -214,7 +214,7 @@ public:
enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0 enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
}; };
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; };
// Define any direct product group to be a model of the multiplicative Group concept // Define any direct product group to be a model of the multiplicative Group concept

67
gtsam/base/make_shared.h Normal file
View File

@ -0,0 +1,67 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file make_shared.h
* @brief make_shared trampoline function to ensure proper alignment
* @author Fan Jiang
*/
#pragma once
#include <gtsam/base/types.h>
#include <Eigen/Core>
#include <boost/make_shared.hpp>
#include <type_traits>
namespace gtsam {
/// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
template<bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;
}
namespace gtsam {
/**
* Add our own `make_shared` as a layer of wrapping on `boost::make_shared`
* This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs
* at runtime, which is notoriously hard to debug.
*
* Explanation
* ===============
* The template `needs_eigen_aligned_allocator<T>::value` will evaluate to `std::true_type` if the type alias
* `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the
* `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro.
*
* This function declaration will only be taken when the above condition is true, so if some object does not need to
* be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for
* `boost::make_shared`.
*
* @tparam T The type of object being constructed
* @tparam Args Type of the arguments of the constructor
* @param args Arguments of the constructor
* @return The object created as a boost::shared_ptr<T>
*/
template<typename T, typename ... Args>
gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
}
/// Fall back to the boost version if no need for alignment
template<typename T, typename ... Args>
gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
return boost::make_shared<T>(std::forward<Args>(args)...);
}
}

View File

@ -42,123 +42,218 @@
namespace gtsam { namespace gtsam {
// Serialization directly to strings in compressed format /** @name Standard serialization
template<class T> * Serialization in default compressed format
std::string serialize(const T& input) { */
std::ostringstream out_archive_stream; ///@{
/// serializes to a stream
template <class T>
void serializeToStream(const T& input, std::ostream& out_archive_stream) {
boost::archive::text_oarchive out_archive(out_archive_stream); boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input; out_archive << input;
return out_archive_stream.str();
} }
template<class T> /// deserializes from a stream
void deserialize(const std::string& serialized, T& output) { template <class T>
std::istringstream in_archive_stream(serialized); void deserializeFromStream(std::istream& in_archive_stream, T& output) {
boost::archive::text_iarchive in_archive(in_archive_stream); boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output; in_archive >> output;
} }
template<class T> /// serializes to a string
template <class T>
std::string serializeToString(const T& input) {
std::ostringstream out_archive_stream;
serializeToStream(input, out_archive_stream);
return out_archive_stream.str();
}
/// deserializes from a string
template <class T>
void deserializeFromString(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
deserializeFromStream(in_archive_stream, output);
}
/// serializes to a file
template <class T>
bool serializeToFile(const T& input, const std::string& filename) { bool serializeToFile(const T& input, const std::string& filename) {
std::ofstream out_archive_stream(filename.c_str()); std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open()) if (!out_archive_stream.is_open()) return false;
return false; serializeToStream(input, out_archive_stream);
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
out_archive_stream.close(); out_archive_stream.close();
return true; return true;
} }
template<class T> /// deserializes from a file
template <class T>
bool deserializeFromFile(const std::string& filename, T& output) { bool deserializeFromFile(const std::string& filename, T& output) {
std::ifstream in_archive_stream(filename.c_str()); std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open()) if (!in_archive_stream.is_open()) return false;
return false; deserializeFromStream(in_archive_stream, output);
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
in_archive_stream.close(); in_archive_stream.close();
return true; return true;
} }
// Serialization to XML format with named structures /// serializes to a string
template<class T> template <class T>
std::string serializeXML(const T& input, const std::string& name="data") { std::string serialize(const T& input) {
std::ostringstream out_archive_stream; return serializeToString(input);
// braces to flush out_archive as it goes out of scope before taking str()
// fixes crash with boost 1.66-1.68
// see https://github.com/boostorg/serialization/issues/82
{
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
return out_archive_stream.str();
} }
template<class T> /// deserializes from a string
void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") { template <class T>
std::istringstream in_archive_stream(serialized); void deserialize(const std::string& serialized, T& output) {
deserializeFromString(serialized, output);
}
///@}
/** @name XML Serialization
* Serialization to XML format with named structures
*/
///@{
/// serializes to a stream in XML
template <class T>
void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
const std::string& name = "data") {
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
/// deserializes from a stream in XML
template <class T>
void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
const std::string& name = "data") {
boost::archive::xml_iarchive in_archive(in_archive_stream); boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output); in_archive >> boost::serialization::make_nvp(name.c_str(), output);
} }
template<class T> /// serializes to a string in XML
bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") { template <class T>
std::ofstream out_archive_stream(filename.c_str()); std::string serializeToXMLString(const T& input,
if (!out_archive_stream.is_open()) const std::string& name = "data") {
return false;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);;
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
in_archive_stream.close();
return true;
}
// Serialization to binary format with named structures
template<class T>
std::string serializeBinary(const T& input, const std::string& name="data") {
std::ostringstream out_archive_stream; std::ostringstream out_archive_stream;
boost::archive::binary_oarchive out_archive(out_archive_stream); serializeToXMLStream(input, out_archive_stream, name);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
return out_archive_stream.str(); return out_archive_stream.str();
} }
template<class T> /// deserializes from a string in XML
void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") { template <class T>
void deserializeFromXMLString(const std::string& serialized, T& output,
const std::string& name = "data") {
std::istringstream in_archive_stream(serialized); std::istringstream in_archive_stream(serialized);
boost::archive::binary_iarchive in_archive(in_archive_stream); deserializeFromXMLStream(in_archive_stream, output, name);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
} }
template<class T> /// serializes to an XML file
bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { template <class T>
bool serializeToXMLFile(const T& input, const std::string& filename,
const std::string& name = "data") {
std::ofstream out_archive_stream(filename.c_str()); std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open()) if (!out_archive_stream.is_open()) return false;
return false; serializeToXMLStream(input, out_archive_stream, name);
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
out_archive_stream.close(); out_archive_stream.close();
return true; return true;
} }
template<class T> /// deserializes from an XML file
bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") { template <class T>
bool deserializeFromXMLFile(const std::string& filename, T& output,
const std::string& name = "data") {
std::ifstream in_archive_stream(filename.c_str()); std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open()) if (!in_archive_stream.is_open()) return false;
return false; deserializeFromXMLStream(in_archive_stream, output, name);
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
in_archive_stream.close(); in_archive_stream.close();
return true; return true;
} }
} // \namespace gtsam /// serializes to a string in XML
template <class T>
std::string serializeXML(const T& input,
const std::string& name = "data") {
return serializeToXMLString(input, name);
}
/// deserializes from a string in XML
template <class T>
void deserializeXML(const std::string& serialized, T& output,
const std::string& name = "data") {
deserializeFromXMLString(serialized, output, name);
}
///@}
/** @name Binary Serialization
* Serialization to binary format with named structures
*/
///@{
/// serializes to a stream in binary
template <class T>
void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
const std::string& name = "data") {
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
/// deserializes from a stream in binary
template <class T>
void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
const std::string& name = "data") {
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
}
/// serializes to a string in binary
template <class T>
std::string serializeToBinaryString(const T& input,
const std::string& name = "data") {
std::ostringstream out_archive_stream;
serializeToBinaryStream(input, out_archive_stream, name);
return out_archive_stream.str();
}
/// deserializes from a string in binary
template <class T>
void deserializeFromBinaryString(const std::string& serialized, T& output,
const std::string& name = "data") {
std::istringstream in_archive_stream(serialized);
deserializeFromBinaryStream(in_archive_stream, output, name);
}
/// serializes to a binary file
template <class T>
bool serializeToBinaryFile(const T& input, const std::string& filename,
const std::string& name = "data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open()) return false;
serializeToBinaryStream(input, out_archive_stream, name);
out_archive_stream.close();
return true;
}
/// deserializes from a binary file
template <class T>
bool deserializeFromBinaryFile(const std::string& filename, T& output,
const std::string& name = "data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open()) return false;
deserializeFromBinaryStream(in_archive_stream, output, name);
in_archive_stream.close();
return true;
}
/// serializes to a string in binary
template <class T>
std::string serializeBinary(const T& input,
const std::string& name = "data") {
return serializeToBinaryString(input, name);
}
/// deserializes from a string in binary
template <class T>
void deserializeBinary(const std::string& serialized, T& output,
const std::string& name = "data") {
deserializeFromBinaryString(serialized, output, name);
}
///@}
} // namespace gtsam

View File

@ -26,6 +26,7 @@
#include <gtsam/base/serialization.h> #include <gtsam/base/serialization.h>
#include <boost/serialization/serialization.hpp> #include <boost/serialization/serialization.hpp>
#include <boost/filesystem.hpp>
// whether to print the serialized text to stdout // whether to print the serialized text to stdout
@ -40,22 +41,37 @@ T create() {
return T(); return T();
} }
// Creates or empties a folder in the build folder and returns the relative path
boost::filesystem::path resetFilesystem(
boost::filesystem::path folder = "actual") {
boost::filesystem::remove_all(folder);
boost::filesystem::create_directory(folder);
return folder;
}
// Templated round-trip serialization // Templated round-trip serialization
template<class T> template<class T>
void roundtrip(const T& input, T& output) { void roundtrip(const T& input, T& output) {
// Serialize
std::string serialized = serialize(input); std::string serialized = serialize(input);
if (verbose) std::cout << serialized << std::endl << std::endl; if (verbose) std::cout << serialized << std::endl << std::endl;
deserialize(serialized, output); deserialize(serialized, output);
} }
// This version requires equality operator // Templated round-trip serialization using a file
template<class T>
void roundtripFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.dat";
serializeToFile(input, path.string());
deserializeFromFile(path.string(), output);
}
// This version requires equality operator and uses string and file round-trips
template<class T> template<class T>
bool equality(const T& input = T()) { bool equality(const T& input = T()) {
T output = create<T>(); T output = create<T>(), outputf = create<T>();
roundtrip<T>(input,output); roundtrip<T>(input,output);
return input==output; roundtripFile<T>(input,outputf);
return (input==output) && (input==outputf);
} }
// This version requires Testable // This version requires Testable
@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) {
// Templated round-trip serialization using XML // Templated round-trip serialization using XML
template<class T> template<class T>
void roundtripXML(const T& input, T& output) { void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized = serializeXML<T>(input); std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl; if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeXML(serialized, output); deserializeXML(serialized, output);
} }
// Templated round-trip serialization using XML File
template<class T>
void roundtripXMLFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.xml";
serializeToXMLFile(input, path.string());
deserializeFromXMLFile(path.string(), output);
}
// This version requires equality operator // This version requires equality operator
template<class T> template<class T>
bool equalityXML(const T& input = T()) { bool equalityXML(const T& input = T()) {
T output = create<T>(); T output = create<T>(), outputf = create<T>();
roundtripXML<T>(input,output); roundtripXML<T>(input,output);
return input==output; roundtripXMLFile<T>(input,outputf);
return (input==output) && (input==outputf);
} }
// This version requires Testable // This version requires Testable
@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) {
// Templated round-trip serialization using XML // Templated round-trip serialization using XML
template<class T> template<class T>
void roundtripBinary(const T& input, T& output) { void roundtripBinary(const T& input, T& output) {
// Serialize
std::string serialized = serializeBinary<T>(input); std::string serialized = serializeBinary<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl; if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeBinary(serialized, output); deserializeBinary(serialized, output);
} }
// Templated round-trip serialization using Binary file
template<class T>
void roundtripBinaryFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.bin";
serializeToBinaryFile(input, path.string());
deserializeFromBinaryFile(path.string(), output);
}
// This version requires equality operator // This version requires equality operator
template<class T> template<class T>
bool equalityBinary(const T& input = T()) { bool equalityBinary(const T& input = T()) {
T output = create<T>(); T output = create<T>(), outputf = create<T>();
roundtripBinary<T>(input,output); roundtripBinary<T>(input,output);
return input==output; roundtripBinaryFile<T>(input,outputf);
return (input==output) && (input==outputf);
} }
// This version requires Testable // This version requires Testable

View File

@ -28,6 +28,7 @@
#include <cstdint> #include <cstdint>
#include <exception> #include <exception>
#include <string>
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
#include <tbb/scalable_allocator.h> #include <tbb/scalable_allocator.h>
@ -54,7 +55,7 @@
namespace gtsam { namespace gtsam {
/// Function to demangle type name of variable, e.g. demangle(typeid(x).name()) /// Function to demangle type name of variable, e.g. demangle(typeid(x).name())
std::string demangle(const char* name); std::string GTSAM_EXPORT demangle(const char* name);
/// Integer nonlinear key type /// Integer nonlinear key type
typedef std::uint64_t Key; typedef std::uint64_t Key;
@ -230,3 +231,50 @@ namespace std {
#ifdef ERROR #ifdef ERROR
#undef ERROR #undef ERROR
#endif #endif
namespace gtsam {
/// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::`
template<typename ...> using void_t = void;
/**
* A SFINAE trait to mark classes that need special alignment.
*
* This is required to make boost::make_shared and etc respect alignment, which is essential for the Python
* wrappers to work properly.
*
* Explanation
* =============
* When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template
* will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`.
*
* Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`.
*
* Please refer to `gtsam/base/make_shared.h` for an example.
*/
template<typename, typename = void_t<>>
struct needs_eigen_aligned_allocator : std::false_type {
};
template<typename T>
struct needs_eigen_aligned_allocator<T, void_t<typename T::_eigen_aligned_allocator_trait>> : std::true_type {
};
}
/**
* This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
* memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
* See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
*/
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
using _eigen_aligned_allocator_trait = void;
/**
* This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
* memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
* See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
*/
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
using _eigen_aligned_allocator_trait = void;

View File

@ -162,7 +162,7 @@ private:
NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0 NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
}; };
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; };
// Declare this to be both Testable and a Manifold // Declare this to be both Testable and a Manifold

View File

@ -319,7 +319,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
template<class CAMERA> template<class CAMERA>

View File

@ -212,7 +212,7 @@ class EssentialMatrix {
/// @} /// @}
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
template<> template<>

View File

@ -325,7 +325,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// manifold traits // manifold traits

View File

@ -222,7 +222,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// end of class PinholeBaseK // end of class PinholeBaseK
@ -425,7 +425,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// end of class PinholePose // end of class PinholePose

View File

@ -317,7 +317,7 @@ public:
public: public:
// Align for Point2, which is either derived from, or is typedef, of Vector2 // Align for Point2, which is either derived from, or is typedef, of Vector2
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; // Pose2 }; // Pose2
/** specialization for pose2 wedge function (generic template in Lie.h) */ /** specialization for pose2 wedge function (generic template in Lie.h) */

View File

@ -355,7 +355,7 @@ public:
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions // Align if we are using Quaternions
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif #endif
}; };
// Pose3 class // Pose3 class

View File

@ -544,7 +544,7 @@ namespace gtsam {
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
// only align if quaternion, Matrix3 has no alignment requirements // only align if quaternion, Matrix3 has no alignment requirements
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif #endif
}; };

View File

@ -20,8 +20,8 @@
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/base/make_shared.h>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <Eigen/Core> #include <Eigen/Core>
#include <iostream> // TODO(frank): how to avoid? #include <iostream> // TODO(frank): how to avoid?
@ -54,7 +54,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>; using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>; using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
protected: protected:
MatrixNN matrix_; ///< Rotation matrix MatrixNN matrix_; ///< Rotation matrix
@ -292,6 +292,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
boost::none) const; boost::none) const;
/// @} /// @}
template <class Archive>
friend void save(Archive&, SO&, const unsigned int);
template <class Archive>
friend void load(Archive&, SO&, const unsigned int);
template <class Archive> template <class Archive>
friend void serialize(Archive&, SO&, const unsigned int); friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access; friend class boost::serialization::access;
@ -329,6 +333,16 @@ template <>
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1, SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const; DynamicJacobian H2) const;
/** Serialization function */
template<class Archive>
void serialize(
Archive& ar, SOn& Q,
const unsigned int file_version
) {
Matrix& M = Q.matrix_;
ar& M;
}
/* /*
* Define the traits. internal::LieGroup provides both Lie group and Testable * Define the traits. internal::LieGroup provides both Lie group and Testable
*/ */

View File

@ -214,7 +214,7 @@ private:
/// @} /// @}
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// Define GTSAM traits // Define GTSAM traits

View File

@ -215,7 +215,7 @@ struct CameraProjectionMatrix {
private: private:
const Matrix3 K_; const Matrix3 K_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
/** /**

View File

@ -139,7 +139,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
/// traits /// traits
@ -219,7 +219,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
/// traits /// traits

View File

@ -100,7 +100,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
@ -210,7 +210,7 @@ public:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
/** /**
@ -332,7 +332,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// class CombinedImuFactor // class CombinedImuFactor

View File

@ -171,7 +171,7 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
/// @} /// @}
}; // ConstantBias class }; // ConstantBias class

View File

@ -69,7 +69,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions // Align if we are using Quaternions
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif #endif
}; };
@ -182,7 +182,7 @@ class GTSAM_EXPORT PreintegratedRotation {
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions // Align if we are using Quaternions
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif #endif
}; };

View File

@ -214,7 +214,7 @@ class GTSAM_EXPORT PreintegrationBase {
#endif #endif
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
} /// namespace gtsam } /// namespace gtsam

View File

@ -84,7 +84,7 @@ protected:
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions // Align if we are using Quaternions
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif #endif
}; };

View File

@ -141,7 +141,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
} /// namespace gtsam } /// namespace gtsam

View File

@ -209,7 +209,7 @@ private:
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; };
// ExpressionFactor // ExpressionFactor

View File

@ -175,7 +175,7 @@ public:
/// @} /// @}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private: private:
@ -265,7 +265,7 @@ public:
traits<X>::Print(value_, "Value"); traits<X>::Print(value_, "Value");
} }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private: private:
@ -331,7 +331,7 @@ public:
return traits<X>::Local(x1,x2); return traits<X>::Local(x1,x2);
} }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private: private:

View File

@ -114,7 +114,7 @@ namespace gtsam {
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; };
} /// namespace gtsam } /// namespace gtsam

View File

@ -169,6 +169,12 @@ class ExecutionTrace {
content.ptr->reverseAD2(dTdA, jacobians); content.ptr->reverseAD2(dTdA, jacobians);
} }
~ExecutionTrace() {
if (kind == Function) {
content.ptr->~CallRecord<Dim>();
}
}
/// Define type so we can apply it as a meta-function /// Define type so we can apply it as a meta-function
typedef ExecutionTrace<T> type; typedef ExecutionTrace<T> type;
}; };

View File

@ -150,7 +150,7 @@ public:
return constant_; return constant_;
} }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------

View File

@ -16,6 +16,7 @@
* @brief unit tests for Expression internals * @brief unit tests for Expression internals
*/ */
#include <gtsam/nonlinear/internal/CallRecord.h>
#include <gtsam/nonlinear/internal/ExecutionTrace.h> #include <gtsam/nonlinear/internal/ExecutionTrace.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>

View File

@ -0,0 +1,84 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
/**
* @file TranslationFactor.h
* @author Frank Dellaert
* @date March 2020
* @brief Binary factor for a relative translation direction measurement.
*/
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Binary factor for a relative translation direction measurement
* w_aZb. The measurement equation is
* w_aZb = Unit3(Tb - Ta)
* i.e., w_aZb is the translation direction from frame A to B, in world
* coordinates. Although Unit3 instances live on a manifold, following
* Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
* ambient world coordinate frame:
* normalized(Tb - Ta) - w_aZb.point3()
*
* @addtogroup SFM
*/
class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
private:
typedef NoiseModelFactor2<Point3, Point3> Base;
Point3 measured_w_aZb_;
public:
/// default constructor
TranslationFactor() {}
TranslationFactor(Key a, Key b, const Unit3& w_aZb,
const SharedNoiseModel& noiseModel)
: Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
/**
* @brief Caclulate error: (norm(Tb - Ta) - measurement)
* where Tb and Ta are Point3 translations and measurement is
* the Unit3 translation direction from a to b.
*
* @param Ta translation for key a
* @param Tb translation for key b
* @param H1 optional jacobian in Ta
* @param H2 optional jacobian in Tb
* @return * Vector
*/
Vector evaluateError(
const Point3& Ta, const Point3& Tb,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
const Point3 dir = Tb - Ta;
Matrix33 H_predicted_dir;
const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
if (H1) *H1 = -H_predicted_dir;
if (H2) *H2 = H_predicted_dir;
return predicted - measured_w_aZb_;
}
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Base", boost::serialization::base_object<Base>(*this));
}
}; // \ TranslationFactor
} // namespace gtsam

View File

@ -0,0 +1,103 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TranslationRecovery.h
* @author Frank Dellaert
* @date March 2020
* @brief test recovering translations when rotations are given.
*/
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <gtsam/slam/PriorFactor.h>
using namespace gtsam;
using namespace std;
NonlinearFactorGraph TranslationRecovery::buildGraph() const {
auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
NonlinearFactorGraph graph;
// Add all relative translation edges
for (auto edge : relativeTranslations_) {
Key a, b;
tie(a, b) = edge.first;
const Unit3 w_aZb = edge.second;
graph.emplace_shared<TranslationFactor>(a, b, w_aZb, noiseModel);
}
return graph;
}
void TranslationRecovery::addPrior(const double scale,
NonlinearFactorGraph* graph) const {
auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
auto edge = relativeTranslations_.begin();
Key a, b;
tie(a, b) = edge->first;
const Unit3 w_aZb = edge->second;
graph->emplace_shared<PriorFactor<Point3> >(a, Point3(0, 0, 0), noiseModel);
graph->emplace_shared<PriorFactor<Point3> >(b, scale * w_aZb.point3(),
noiseModel);
}
Values TranslationRecovery::initalizeRandomly() const {
// Create a lambda expression that checks whether value exists and randomly
// initializes if not.
Values initial;
auto insert = [&initial](Key j) {
if (!initial.exists(j)) {
initial.insert<Point3>(j, Vector3::Random());
}
};
// Loop over measurements and add a random translation
for (auto edge : relativeTranslations_) {
Key a, b;
tie(a, b) = edge.first;
insert(a);
insert(b);
}
return initial;
}
Values TranslationRecovery::run(const double scale) const {
auto graph = buildGraph();
addPrior(scale, &graph);
const Values initial = initalizeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize();
return result;
}
TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
const Values& poses, const vector<KeyPair>& edges) {
TranslationEdges relativeTranslations;
for (auto edge : edges) {
Key a, b;
tie(a, b) = edge;
const Pose3 wTa = poses.at<Pose3>(a), wTb = poses.at<Pose3>(b);
const Point3 Ta = wTa.translation(), Tb = wTb.translation();
const Unit3 w_aZb(Tb - Ta);
relativeTranslations[edge] = w_aZb;
}
return relativeTranslations;
}

View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TranslationRecovery.h
* @author Frank Dellaert
* @date March 2020
* @brief test recovering translations when rotations are given.
*/
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <map>
#include <utility>
namespace gtsam {
// Set up an optimization problem for the unknown translations Ti in the world
// coordinate frame, given the known camera attitudes wRi with respect to the
// world frame, and a set of (noisy) translation directions of type Unit3,
// w_aZb. The measurement equation is
// w_aZb = Unit3(Tb - Ta) (1)
// i.e., w_aZb is the translation direction from frame A to B, in world
// coordinates. Although Unit3 instances live on a manifold, following
// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
// ambient world coordinate frame.
//
// It is clear that we cannot recover the scale, nor the absolute position,
// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
// the translations Ta and Tb associated with the first measurement w_aZb,
// clamping them to their initial values as given to this method. If no initial
// values are given, we use the origin for Tb and set Tb to make (1) come
// through, i.e.,
// Tb = s * wRa * Point3(w_aZb) (2)
// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
// versions are supplied below corresponding to whether we have initial values
// or not.
class TranslationRecovery {
public:
using KeyPair = std::pair<Key, Key>;
using TranslationEdges = std::map<KeyPair, Unit3>;
private:
TranslationEdges relativeTranslations_;
LevenbergMarquardtParams params_;
public:
/**
* @brief Construct a new Translation Recovery object
*
* @param relativeTranslations the relative translations, in world coordinate
* frames, indexed in a map by a pair of Pose keys.
* @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
* used to modify the parameters for the LM optimizer. By default, uses the
* default LM parameters.
*/
TranslationRecovery(const TranslationEdges& relativeTranslations,
const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams())
: relativeTranslations_(relativeTranslations), params_(lmParams) {
params_.setVerbosityLM("Summary");
}
/**
* @brief Build the factor graph to do the optimization.
*
* @return NonlinearFactorGraph
*/
NonlinearFactorGraph buildGraph() const;
/**
* @brief Add priors on ednpoints of first measurement edge.
*
* @param scale scale for first relative translation which fixes gauge.
* @param graph factor graph to which prior is added.
*/
void addPrior(const double scale, NonlinearFactorGraph* graph) const;
/**
* @brief Create random initial translations.
*
* @return Values
*/
Values initalizeRandomly() const;
/**
* @brief Build and optimize factor graph.
*
* @param scale scale for first relative translation which fixes gauge.
* @return Values
*/
Values run(const double scale = 1.0) const;
/**
* @brief Simulate translation direction measurements
*
* @param poses SE(3) ground truth poses stored as Values
* @param edges pairs (a,b) for which a measurement w_aZb will be generated.
* @return TranslationEdges map from a KeyPair to the simulated Unit3
* translation direction measurement between the cameras in KeyPair.
*/
static TranslationEdges SimulateMeasurements(
const Values& poses, const std::vector<KeyPair>& edges);
};
} // namespace gtsam

View File

@ -0,0 +1,108 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testTranslationFactor.cpp
* @brief Unit tests for TranslationFactor Class
* @author Frank dellaert
* @date March 2020
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
// Create a noise model for the chordal error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05));
// Keys are deliberately *not* in sorted order to test that case.
static const Key kKey1(2), kKey2(1);
static const Unit3 kMeasured(1, 0, 0);
/* ************************************************************************* */
TEST(TranslationFactor, Constructor) {
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
}
/* ************************************************************************* */
TEST(TranslationFactor, ZeroError) {
// Create a factor
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
// Set the linearization
Point3 T1(1, 0, 0), T2(2, 0, 0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(T1, T2));
// Verify we get the expected error
Vector expected = (Vector3() << 0, 0, 0).finished();
EXPECT(assert_equal(expected, actualError, 1e-9));
}
/* ************************************************************************* */
TEST(TranslationFactor, NonZeroError) {
// create a factor
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
// set the linearization
Point3 T1(0, 1, 1), T2(0, 2, 2);
// use the factor to calculate the error
Vector actualError(factor.evaluateError(T1, T2));
// verify we get the expected error
Vector expected = (Vector3() << -1, 1/sqrt(2), 1/sqrt(2)).finished();
EXPECT(assert_equal(expected, actualError, 1e-9));
}
/* ************************************************************************* */
Vector factorError(const Point3& T1, const Point3& T2,
const TranslationFactor& factor) {
return factor.evaluateError(T1, T2);
}
TEST(TranslationFactor, Jacobian) {
// Create a factor
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
// Set the linearization
Point3 T1(1, 0, 0), T2(2, 0, 0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(T1, T2, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError, _1, T2, factor), T1);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError, T1, _1, factor), T2);
// Verify the Jacobians are correct
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

View File

@ -126,7 +126,7 @@ namespace gtsam {
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; // \class BetweenFactor }; // \class BetweenFactor
/// traits /// traits

View File

@ -105,7 +105,7 @@ private:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// \class EssentialMatrixConstraint // \class EssentialMatrixConstraint

View File

@ -81,7 +81,7 @@ public:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
/** /**
@ -201,7 +201,7 @@ public:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// EssentialMatrixFactor2 // EssentialMatrixFactor2
@ -286,7 +286,7 @@ public:
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// EssentialMatrixFactor3 // EssentialMatrixFactor3

View File

@ -0,0 +1,117 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Various factors that minimize some Frobenius norm
*/
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/base/timing.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <cmath>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
//******************************************************************************
boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit) {
double sigma = 1.0;
if (model != nullptr) {
if (model->dim() != 6) {
if (!defaultToUnit)
throw std::runtime_error("Can only convert Pose3 noise models");
} else {
auto sigmas = model->sigmas().head(3).eval();
if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) {
if (!defaultToUnit)
throw std::runtime_error("Can only convert isotropic rotation noise");
} else {
sigma = sigmas(0);
}
}
}
return noiseModel::Isotropic::Sigma(d, sigma);
}
//******************************************************************************
FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12,
size_t p,
const SharedNoiseModel& model)
: NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
M_(R12.matrix()), // 3*3 in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
dimension_(SOn::Dimension(p)), // 6 for SO(4)
G_(pp_, dimension_) // 16*6 for SO(4)
{
// Calculate G matrix of vectorized generators
Matrix Z = Matrix::Zero(p, p);
for (size_t j = 0; j < dimension_; j++) {
const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j));
G_.col(j) = Eigen::Map<const Matrix>(X.data(), pp_, 1);
}
assert(noiseModel()->dim() == 3 * p_);
}
//******************************************************************************
Vector FrobeniusWormholeFactor::evaluateError(
const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
gttic(FrobeniusWormholeFactorP_evaluateError);
const Matrix& M1 = Q1.matrix();
const Matrix& M2 = Q2.matrix();
assert(M1.rows() == p_ && M2.rows() == p_);
const size_t dim = 3 * p_; // Stiefel manifold dimension
Vector fQ2(dim), hQ1(dim);
// Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
// Vectorize M1*P*R12
const Matrix Q1PR12 = M1.leftCols<3>() * M_;
hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
// If asked, calculate Jacobian as (M \otimes M1) * G
if (H1) {
const size_t p2 = 2 * p_;
Matrix RPxQ = Matrix::Zero(dim, pp_);
RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0);
RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1);
RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2);
*H1 = -RPxQ * G_;
}
if (H2) {
const size_t p2 = 2 * p_;
Matrix PxQ = Matrix::Zero(dim, pp_);
PxQ.block(0, 0, p_, p_) = M2;
PxQ.block(p_, p_, p_, p_) = M2;
PxQ.block(p2, p2, p_, p_) = M2;
*H2 = PxQ * G_;
}
return fQ2 - hQ1;
}
//******************************************************************************
} // namespace gtsam

View File

@ -0,0 +1,145 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FrobeniusFactor.h
* @date March 2019
* @author Frank Dellaert
* @brief Various factors that minimize some Frobenius norm
*/
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
* BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is
* null we return a Dim-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the 3-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input.
*/
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
/**
* FrobeniusPrior calculates the Frobenius norm between a given matrix and an
* element of SO(3) or SO(4).
*/
template <class Rot>
class FrobeniusPrior : public NoiseModelFactor1<Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
using MatrixNN = typename Rot::MatrixNN;
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
public:
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) {
vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
}
/// Error is just Frobenius norm between Rot element and vectorized matrix M.
Vector evaluateError(const Rot& R,
boost::optional<Matrix&> H = boost::none) const {
return R.vec(H) - vecM_; // Jacobian is computed only when needed.
}
};
/**
* FrobeniusFactor calculates the Frobenius norm between rotation matrices.
* The template argument can be any fixed-size SO<N>.
*/
template <class Rot>
class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1,
j2) {}
/// 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 {
Vector error = R2.vec(H2) - R1.vec(H1);
if (H1) *H1 = -*H1;
return error;
}
};
/**
* FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm
* of the rotation error between measured and predicted (rather than the
* Logmap of the error). This factor is only defined for fixed-dimension types,
* and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
*/
template <class Rot>
class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
Rot R12_; ///< measured rotation between R1 and R2
Eigen::Matrix<double, Rot::dimension, Rot::dimension>
R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(
ConvertPose3NoiseModel(model, Dim), j1, j2),
R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
/// 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 {
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);
if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
return error;
}
};
/**
* FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will
* land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
* TODO(frank): template on D=2 or 3
*/
class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_, dimension_; ///< dimensionality constants
Matrix G_; ///< matrix of vectorized generators
public:
/// Constructor. Note we convert to 3*p-dimensional noise model.
FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4,
const SharedNoiseModel& model = nullptr);
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// 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;
};
} // namespace gtsam

View File

@ -189,7 +189,7 @@ namespace gtsam {
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
/// traits /// traits

View File

@ -113,7 +113,7 @@ public:
return error; return error;
} }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
} // namespace gtsam } // namespace gtsam

View File

@ -81,7 +81,7 @@ protected:
mutable FBlocks Fs; mutable FBlocks Fs;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;

View File

@ -0,0 +1,244 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* testFrobeniusFactor.cpp
*
* @file testFrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Check evaluateError for various Frobenius norm
*/
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
namespace so3 {
SO3 id;
Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1);
Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace so3
/* ************************************************************************* */
TEST(FrobeniusPriorSO3, evaluateError) {
using namespace ::so3;
auto factor = FrobeniusPrior<SO3>(1, R2.matrix());
Vector actual = factor.evaluateError(R1);
Vector expected = R1.vec() - R2.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, R1);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(FrobeniusPriorSO3, ClosestTo) {
// Example top-left of SO(4) matrix not quite on SO(3) manifold
Matrix3 M;
M << 0.79067393, 0.6051136, -0.0930814, //
0.4155925, -0.64214347, -0.64324489, //
-0.44948549, 0.47046326, -0.75917576;
SO3 expected = SO3::ClosestTo(M);
// manifold optimization gets same result as SVD solution in ClosestTo
NonlinearFactorGraph graph;
graph.emplace_shared<FrobeniusPrior<SO3> >(1, M);
Values initial;
initial.insert(1, SO3(I_3x3));
auto result = GaussNewtonOptimizer(graph, initial).optimize();
EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-6);
EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-6));
}
/* ************************************************************************* */
TEST(FrobeniusPriorSO3, ChordalL2mean) {
// See Hartley13ijcv:
// Cost function C(R) = \sum FrobeniusPrior(R,R_i)
// Closed form solution = ClosestTo(C_e), where
// C_e = \sum R_i !!!!
// We will test by computing mean of R1=exp(v1) R1^T=exp(-v1):
using namespace ::so3;
SO3 expected; // identity
Matrix3 M = R1.matrix() + R1.matrix().transpose();
EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6));
EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6));
// manifold optimization gets same result as ChordalMean
NonlinearFactorGraph graph;
graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix());
graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix().transpose());
Values initial;
initial.insert<SO3>(1, R1.inverse());
auto result = GaussNewtonOptimizer(graph, initial).optimize();
EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1); // Why so loose?
EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-5));
}
/* ************************************************************************* */
TEST(FrobeniusFactorSO3, evaluateError) {
using namespace ::so3;
auto factor = FrobeniusFactor<SO3>(1, 2);
Vector actual = factor.evaluateError(R1, R2);
Vector expected = R2.vec() - R1.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, R1);
values.insert(2, R2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
// Commented out as SO(n) not yet supported (and might never be)
// TEST(FrobeniusBetweenFactorSOn, evaluateError) {
// using namespace ::so3;
// auto factor =
// FrobeniusBetweenFactor<SOn>(1, 2, SOn::FromMatrix(R12.matrix()));
// Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()),
// SOn::FromMatrix(R2.matrix()));
// Vector expected = Vector9::Zero();
// EXPECT(assert_equal(expected, actual, 1e-9));
// Values values;
// values.insert(1, R1);
// values.insert(2, R2);
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
// }
/* ************************************************************************* */
TEST(FrobeniusBetweenFactorSO3, evaluateError) {
using namespace ::so3;
auto factor = FrobeniusBetweenFactor<SO3>(1, 2, R12);
Vector actual = factor.evaluateError(R1, R2);
Vector expected = Vector9::Zero();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, R1);
values.insert(2, R2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
//******************************************************************************
namespace so4 {
SO4 id;
Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
SO4 Q2 = SO4::Expmap(v2);
} // namespace so4
/* ************************************************************************* */
TEST(FrobeniusFactorSO4, evaluateError) {
using namespace ::so4;
auto factor = FrobeniusFactor<SO4>(1, 2, noiseModel::Unit::Create(6));
Vector actual = factor.evaluateError(Q1, Q2);
Vector expected = Q2.vec() - Q1.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(FrobeniusBetweenFactorSO4, evaluateError) {
using namespace ::so4;
Matrix4 M{I_4x4};
M.topLeftCorner<3, 3>() = ::so3::R12.matrix();
auto factor = FrobeniusBetweenFactor<SO4>(1, 2, Q1.between(Q2));
Matrix H1, H2;
Vector actual = factor.evaluateError(Q1, Q2, H1, H2);
Vector expected = SO4::VectorN2::Zero();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
//******************************************************************************
namespace submanifold {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1.tail<3>());
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2.tail<3>());
SO4 Q2 = SO4::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace submanifold
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // dimension = 6 not 16
for (const size_t p : {5, 4, 3}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(3, 3) = submanifold::R1.matrix();
SOn Q1(M);
M.topLeftCorner(3, 3) = submanifold::R2.matrix();
SOn Q2(M);
auto factor =
FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, equivalenceToSO3) {
using namespace ::submanifold;
auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension
auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model);
const Matrix3 E3(factor3.evaluateError(R1, R2).data());
const Matrix43 E4(
factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

View File

@ -9,5 +9,6 @@ set (GTSAM_USE_TBB @GTSAM_USE_TBB@)
set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@)
if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") if("@GTSAM_INSTALL_CYTHON_TOOLBOX@")
list(APPEND GTSAM_CYTHON_INSTALL_PATH "@GTSAM_CYTHON_INSTALL_PATH@")
list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@")
endif() endif()

View File

@ -0,0 +1,90 @@
/**
* @file PoseToPointFactor.hpp
* @brief This factor can be used to track a 3D landmark over time by
*providing local measurements of its location.
* @author David Wisth
**/
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <ostream>
namespace gtsam {
/**
* A class for a measurement between a pose and a point.
* @addtogroup SLAM
*/
class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
private:
typedef PoseToPointFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base;
Point3 measured_; /** the point measurement in local coordinates */
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
/** default constructor - only use for serialization */
PoseToPointFactor() {}
/** Constructor */
PoseToPointFactor(Key key1, Key key2, const Point3& measured,
const SharedNoiseModel& model)
: Base(model, key1, key2), measured_(measured) {}
virtual ~PoseToPointFactor() {}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors
* @brief Error = wTwi.inverse()*wPwp - measured_
* @param wTwi The pose of the sensor in world coordinates
* @param wPwp The estimated point location in world coordinates
*
* Note: measured_ and the error are in local coordiantes.
*/
Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return wTwi.transformTo(wPwp, H1, H2) - measured_;
}
/** return the measured */
const Point3& measured() const { return measured_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class PoseToPointFactor
} // namespace gtsam

View File

@ -0,0 +1,86 @@
/**
* @file testPoseToPointFactor.cpp
* @brief
* @author David Wisth
* @date June 20, 2020
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/slam/PoseToPointFactor.h>
using namespace gtsam;
using namespace gtsam::noiseModel;
/// Verify zero error when there is no noise
TEST(PoseToPointFactor, errorNoiseless) {
Pose3 pose = Pose3::identity();
Point3 point(1.0, 2.0, 3.0);
Point3 noise(0.0, 0.0, 0.0);
Point3 measured = t + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor factor(pose_key, point_key, measured,
Isotropic::Sigma(3, 0.05));
Vector expectedError = Vector3(0.0, 0.0, 0.0);
Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/// Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise) {
Pose3 pose = Pose3::identity();
Point3 point(1.0, 2.0, 3.0);
Point3 noise(-1.0, 0.5, 0.3);
Point3 measured = t + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor factor(pose_key, point_key, measured,
Isotropic::Sigma(3, 0.05));
Vector expectedError = noise;
Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/// Check Jacobians are correct
TEST(PoseToPointFactor, jacobian) {
// Measurement
gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
// Linearisation point
gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
Pose3 p(p_R, p_t);
gtsam::Point3 l = gtsam::Point3(3, 0, 5);
// Factor
Key pose_key(1);
Key point_key(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives
auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
boost::none, boost::none);
Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
// Use the factor to calculate the derivative
Matrix actual_H1;
Matrix actual_H2;
factor.evaluateError(p, l, actual_H1, actual_H2);
// Verify we get the expected error
EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8));
EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

View File

@ -0,0 +1,87 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testTranslationRecovery.cpp
* @author Frank Dellaert
* @date March 2020
* @brief test recovering translations when rotations are given.
*/
#include <gtsam/sfm/TranslationRecovery.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/dataset.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
// We read the BAL file, which has 3 cameras in it, with poses. We then assume
// the rotations are correct, but translations have to be estimated from
// translation directions only. Since we have 3 cameras, A, B, and C, we can at
// most create three relative measurements, let's call them w_aZb, w_aZc, and
// bZc. These will be of type Unit3. We then call `recoverTranslations` which
// sets up an optimization problem for the three unknown translations.
TEST(TranslationRecovery, BAL) {
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData db;
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
// Get camera poses, as Values
size_t j = 0;
Values poses;
for (auto camera : db.cameras) {
poses.insert(j++, camera.pose());
}
// Simulate measurements
const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {0, 2}, {1, 2}});
// Check
const Pose3 wTa = poses.at<Pose3>(0), wTb = poses.at<Pose3>(1),
wTc = poses.at<Pose3>(2);
const Point3 Ta = wTa.translation(), Tb = wTb.translation(),
Tc = wTc.translation();
const Rot3 aRw = wTa.rotation().inverse();
const Unit3 w_aZb = relativeTranslations.at({0, 1});
EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
const Unit3 w_aZc = relativeTranslations.at({0, 2});
EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc));
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
// Translation recovery, version 1
const double scale = 2.0;
const auto result = algorithm.run(scale);
// Check result for first two translations, determined by prior
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at<Point3>(1)));
// Check that the third translations is correct
Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
// TODO(frank): how to get stats back?
// EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

View File

@ -0,0 +1,110 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file timeFrobeniusFactor.cpp
* @brief time FrobeniusFactor with BAL file
* @author Frank Dellaert
* @date June 6, 2015
*/
#include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <iostream>
#include <string>
#include <vector>
using namespace std;
using namespace gtsam;
static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
int main(int argc, char* argv[]) {
// primitive argument parsing:
if (argc > 3) {
throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]");
}
string g2oFile;
try {
if (argc > 1)
g2oFile = argv[argc - 1];
else
g2oFile =
"/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/"
"datasets/randomTorus3D.g2o";
// g2oFile = findExampleDataFile("sphere_smallnoise.graph");
} catch (const exception& e) {
cerr << e.what() << '\n';
exit(1);
}
// Read G2O file
const auto factors = parse3DFactors(g2oFile);
const auto poses = parse3DPoses(g2oFile);
// Build graph
NonlinearFactorGraph graph;
// graph.add(NonlinearEquality<SO4>(0, SO4()));
auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
graph.add(PriorFactor<SO4>(0, SO4(), priorModel));
for (const auto& factor : factors) {
const auto& keys = factor->keys();
const auto& Tij = factor->measured();
const auto& model = factor->noiseModel();
graph.emplace_shared<FrobeniusWormholeFactor>(
keys[0], keys[1], SO3(Tij.rotation().matrix()), model);
}
boost::mt19937 rng(42);
// Set parameters to be similar to ceres
LevenbergMarquardtParams params;
LevenbergMarquardtParams::SetCeresDefaults(&params);
params.setLinearSolverType("MULTIFRONTAL_QR");
// params.setVerbosityLM("SUMMARY");
// params.linearSolverType = LevenbergMarquardtParams::Iterative;
// auto pcg = boost::make_shared<PCGSolverParameters>();
// pcg->preconditioner_ =
// boost::make_shared<SubgraphPreconditionerParameters>();
// boost::make_shared<BlockJacobiPreconditionerParameters>();
// params.iterativeParams = pcg;
// Optimize
for (size_t i = 0; i < 100; i++) {
gttic_(optimize);
Values initial;
initial.insert(0, SO4());
for (size_t j = 1; j < poses.size(); j++) {
initial.insert(j, SO4::Random(rng));
}
LevenbergMarquardtOptimizer lm(graph, initial, params);
Values result = lm.optimize();
cout << "cost = " << graph.error(result) << endl;
}
tictoc_finishedIteration_();
tictoc_print_();
return 0;
}

View File

@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
" T* get()\n" " T* get()\n"
" long use_count() const\n" " long use_count() const\n"
" T& operator*()\n\n" " T& operator*()\n\n"
" cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n" " cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n";
// gtsam alignment-friendly shared_ptr
pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n"
" cdef shared_ptr[T] make_shared[T](const T& r)\n\n"; " cdef shared_ptr[T] make_shared[T](const T& r)\n\n";
for(const TypedefPair& types: typedefs) for(const TypedefPair& types: typedefs)

View File

@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost":
T& operator*() T& operator*()
cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r) cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)
cdef extern from "gtsam/base/make_shared.h" namespace "gtsam":
cdef shared_ptr[T] make_shared[T](const T& r) cdef shared_ptr[T] make_shared[T](const T& r)
cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam": cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam":