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

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

release/4.3a0
Fan Jiang 2020-07-09 17:07:07 -04:00
parent 4a0b031a2a 0f0752fd1f
commit 686ea137e7
57 changed files with 1794 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.travis.yml
View File

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

56
cython/gtsam/tests/test_FrobeniusFactor.py Normal file
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()

20
cython/gtsam/tests/test_logging_optimizer.py
View File

@ -4,6 +4,12 @@ Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
import sys
if sys.version_info.major >= 3:
from io import StringIO
else:
from cStringIO import StringIO
import unittest
from datetime import datetime
@ -37,11 +43,19 @@ class TestOptimizeComet(GtsamTestCase):
self.optimizer = gtsam.GaussNewtonOptimizer(
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):
"""Test with a simple hook."""
# Provide a hook that just prints
def hook(_, error: float):
def hook(_, error):
print(error)
# 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))
# I want to do some comet thing here
def hook(optimizer, error: float):
def hook(optimizer, error):
comet.log_metric("Karcher error",
error, optimizer.iterations())
@ -76,4 +90,4 @@ class TestOptimizeComet(GtsamTestCase):
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
if __name__ == "__main__":
unittest.main()
unittest.main()

10
cython/gtsam/utils/logging_optimizer.py
View File

@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
from typing import TypeVar
from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
import gtsam
T = TypeVar('T')
def optimize(optimizer: T, check_convergence, hook):
def optimize(optimizer, check_convergence, hook):
""" Given an optimizer and a convergence check, iterate until convergence.
After each iteration, hook(optimizer, error) is called.
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
def gtsam_optimize(optimizer: NonlinearOptimizer,
params: NonlinearOptimizerParams,
def gtsam_optimize(optimizer,
params,
hook):
""" Given an optimizer and params, iterate until convergence.
After each iteration, hook(optimizer) is called.

2
cython/requirements.txt
View File

@ -1,3 +1,3 @@
Cython>=0.25.2
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>
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 {
void serializable() const;
};
@ -598,6 +598,7 @@ class SOn {
// Standard Constructors
SOn(size_t n);
static gtsam::SOn FromMatrix(Matrix R);
static gtsam::SOn Lift(size_t n, Matrix R);
// Testable
void print(string s) const;
@ -2835,6 +2836,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
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
//*************************************************************************
@ -2955,6 +2984,7 @@ class PreintegratedImuMeasurements {
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
@ -3016,6 +3046,7 @@ class PreintegratedCombinedMeasurements {
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;

2
gtsam/3rdparty/CMakeLists.txt vendored
View File

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

2
gtsam/base/GenericValue.h
View File

@ -181,7 +181,7 @@ public:
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
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

2
gtsam/base/Manifold.h
View File

@ -214,7 +214,7 @@ public:
enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
};
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

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)...);
}
}

239
gtsam/base/serialization.h
View File

@ -42,123 +42,218 @@
namespace gtsam {
// Serialization directly to strings in compressed format
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
/** @name Standard serialization
* Serialization in default compressed format
*/
///@{
/// 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);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
/// deserializes from a stream
template <class T>
void deserializeFromStream(std::istream& in_archive_stream, T& output) {
boost::archive::text_iarchive in_archive(in_archive_stream);
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) {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
if (!out_archive_stream.is_open()) return false;
serializeToStream(input, out_archive_stream);
out_archive_stream.close();
return true;
}
template<class T>
/// deserializes from a file
template <class T>
bool deserializeFromFile(const std::string& filename, T& output) {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
if (!in_archive_stream.is_open()) return false;
deserializeFromStream(in_archive_stream, output);
in_archive_stream.close();
return true;
}
// Serialization to XML format with named structures
template<class T>
std::string serializeXML(const T& input, const std::string& name="data") {
std::ostringstream out_archive_stream;
// 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();
/// serializes to a string
template <class T>
std::string serialize(const T& input) {
return serializeToString(input);
}
template<class T>
void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
/// deserializes from a string
template <class T>
void deserialize(const std::string& serialized, T& output) {
deserializeFromString(serialized, output);
}
///@}
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());
if (!out_archive_stream.is_open())
return false;
/** @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);;
out_archive_stream.close();
return true;
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
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;
/// 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);
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") {
/// serializes to a string in XML
template <class T>
std::string serializeToXMLString(const T& input,
const std::string& name = "data") {
std::ostringstream out_archive_stream;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
serializeToXMLStream(input, out_archive_stream, name);
return out_archive_stream.str();
}
template<class T>
void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") {
/// deserializes from a string in XML
template <class T>
void deserializeFromXMLString(const std::string& serialized, T& output,
const std::string& name = "data") {
std::istringstream in_archive_stream(serialized);
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
deserializeFromXMLStream(in_archive_stream, output, name);
}
template<class T>
bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
/// serializes to an XML file
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());
if (!out_archive_stream.is_open())
return false;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
if (!out_archive_stream.is_open()) return false;
serializeToXMLStream(input, out_archive_stream, name);
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") {
/// deserializes from an XML file
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::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
if (!in_archive_stream.is_open()) return false;
deserializeFromXMLStream(in_archive_stream, output, name);
in_archive_stream.close();
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

58
gtsam/base/serializationTestHelpers.h
View File

@ -26,6 +26,7 @@
#include <gtsam/base/serialization.h>
#include <boost/serialization/serialization.hpp>
#include <boost/filesystem.hpp>
// whether to print the serialized text to stdout
@ -40,22 +41,37 @@ T create() {
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
template<class T>
void roundtrip(const T& input, T& output) {
// Serialize
std::string serialized = serialize(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
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>
bool equality(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtrip<T>(input,output);
return input==output;
roundtripFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable
@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) {
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
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
template<class T>
bool equalityXML(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtripXML<T>(input,output);
return input==output;
roundtripXMLFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable
@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) {
// Templated round-trip serialization using XML
template<class T>
void roundtripBinary(const T& input, T& output) {
// Serialize
std::string serialized = serializeBinary<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
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
template<class T>
bool equalityBinary(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtripBinary<T>(input,output);
return input==output;
roundtripBinaryFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable

50
gtsam/base/types.h
View File

@ -28,6 +28,7 @@
#include <cstdint>
#include <exception>
#include <string>
#ifdef GTSAM_USE_TBB
#include <tbb/scalable_allocator.h>
@ -54,7 +55,7 @@
namespace gtsam {
/// 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
typedef std::uint64_t Key;
@ -230,3 +231,50 @@ namespace std {
#ifdef ERROR
#undef ERROR
#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;

2
gtsam/geometry/BearingRange.h
View File

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

2
gtsam/geometry/CameraSet.h
View File

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

2
gtsam/geometry/EssentialMatrix.h
View File

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

2
gtsam/geometry/PinholeCamera.h
View File

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

4
gtsam/geometry/PinholePose.h
View File

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

2
gtsam/geometry/Pose2.h
View File

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

2
gtsam/geometry/Pose3.h
View File

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

2
gtsam/geometry/Rot3.h
View File

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

18
gtsam/geometry/SOn.h
View File

@ -20,8 +20,8 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/make_shared.h>
#include <gtsam/dllexport.h>
#include <Eigen/Core>
#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 MatrixDD = Eigen::Matrix<double, dimension, dimension>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
protected:
MatrixNN matrix_; ///< Rotation matrix
@ -292,6 +292,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
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>
friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access;
@ -329,6 +333,16 @@ template <>
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
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
*/

2
gtsam/geometry/Unit3.h
View File

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

2
gtsam/geometry/triangulation.h
View File

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

4
gtsam/navigation/AttitudeFactor.h
View File

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

6
gtsam/navigation/CombinedImuFactor.h
View File

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

2
gtsam/navigation/ImuBias.h
View File

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

4
gtsam/navigation/PreintegratedRotation.h
View File

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

2
gtsam/navigation/PreintegrationBase.h
View File

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

2
gtsam/navigation/PreintegrationParams.h
View File

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

2
gtsam/navigation/TangentPreintegration.h
View File

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

2
gtsam/nonlinear/ExpressionFactor.h
View File

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

6
gtsam/nonlinear/NonlinearEquality.h
View File

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

2
gtsam/nonlinear/PriorFactor.h
View File

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

6
gtsam/nonlinear/internal/ExecutionTrace.h
View File

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

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

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

1
gtsam/nonlinear/tests/testExecutionTrace.cpp
View File

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

84
gtsam/sfm/TranslationFactor.h Normal file
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

103
gtsam/sfm/TranslationRecovery.cpp Normal file
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;
}

114
gtsam/sfm/TranslationRecovery.h Normal file
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

108
gtsam/sfm/tests/testTranslationFactor.cpp Normal file
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);
}
/* ************************************************************************* */

2
gtsam/slam/BetweenFactor.h
View File

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

2
gtsam/slam/EssentialMatrixConstraint.h
View File

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

6
gtsam/slam/EssentialMatrixFactor.h
View File

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

117
gtsam/slam/FrobeniusFactor.cpp Normal file
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

145
gtsam/slam/FrobeniusFactor.h Normal file
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

2
gtsam/slam/ProjectionFactor.h
View File

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

2
gtsam/slam/RotateFactor.h
View File

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

2
gtsam/slam/SmartFactorBase.h
View File

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

244
gtsam/slam/tests/testFrobeniusFactor.cpp Normal file
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);
}
/* ************************************************************************* */

1
gtsam_extra.cmake.in
View File

@ -9,5 +9,6 @@ set (GTSAM_USE_TBB @GTSAM_USE_TBB@)
set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@)
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@")
endif()

90
gtsam_unstable/slam/PoseToPointFactor.h Normal file
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

86
gtsam_unstable/slam/tests/testPoseToPointFactor.h Normal file
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);
}
/* ************************************************************************* */

87
tests/testTranslationRecovery.cpp Normal file
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);
}
/* ************************************************************************* */

110
timing/timeFrobeniusFactor.cpp Normal file
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;
}

5
wrap/Module.cpp
View File

@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
" T* get()\n"
" long use_count() const\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";
for(const TypedefPair& types: typedefs)

2
wrap/tests/expected-cython/geometry.pxd
View File

@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost":
T& operator*()
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 extern from "gtsam/geometry/Point2.h" namespace "gtsam":