Merge branch 'develop' into fix-examples
Varun Agrawal
commit
17bf352e26
|
|
@ -52,6 +52,8 @@ function configure()
|
|||
-DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
|
||||
-DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
|
||||
-DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
|
||||
-DGTSAM_USE_BOOST_FEATURES=${GTSAM_USE_BOOST_FEATURES:-ON} \
|
||||
-DGTSAM_ENABLE_BOOST_SERIALIZATION=${GTSAM_ENABLE_BOOST_SERIALIZATION:-ON} \
|
||||
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
|
||||
-DGTSAM_SINGLE_TEST_EXE=OFF
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,10 +25,11 @@ jobs:
|
|||
# Github Actions requires a single row to be added to the build matrix.
|
||||
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
|
||||
name: [
|
||||
# "Bracket" the versions from GCC [9-14] and Clang [9-16]
|
||||
ubuntu-20.04-gcc-9,
|
||||
ubuntu-20.04-clang-9,
|
||||
ubuntu-22.04-gcc-12,
|
||||
ubuntu-22.04-clang-14,
|
||||
ubuntu-24.04-gcc-14,
|
||||
ubuntu-24.04-clang-16,
|
||||
]
|
||||
|
||||
build_type: [Debug, Release]
|
||||
|
|
@ -44,16 +45,16 @@ jobs:
|
|||
compiler: clang
|
||||
version: "9"
|
||||
|
||||
- name: ubuntu-22.04-gcc-12
|
||||
os: ubuntu-22.04
|
||||
- name: ubuntu-24.04-gcc-14
|
||||
os: ubuntu-24.04
|
||||
compiler: gcc
|
||||
version: "11"
|
||||
|
||||
- name: ubuntu-22.04-clang-14
|
||||
os: ubuntu-22.04
|
||||
compiler: clang
|
||||
version: "14"
|
||||
|
||||
- name: ubuntu-24.04-clang-16
|
||||
os: ubuntu-24.04
|
||||
compiler: clang
|
||||
version: "16"
|
||||
|
||||
steps:
|
||||
- name: Checkout
|
||||
uses: actions/checkout@v4
|
||||
|
|
|
|||
|
|
@ -115,19 +115,24 @@ jobs:
|
|||
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
|
||||
fi
|
||||
|
||||
- name: Install Boost
|
||||
if: runner.os == 'Linux'
|
||||
run: |
|
||||
sudo apt-get -y install libboost-all-dev
|
||||
|
||||
- name: Install (macOS)
|
||||
if: runner.os == 'macOS'
|
||||
run: |
|
||||
brew install cmake ninja boost
|
||||
brew install cmake ninja
|
||||
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
|
||||
echo "CC=clang" >> $GITHUB_ENV
|
||||
echo "CXX=clang++" >> $GITHUB_ENV
|
||||
|
||||
- name: Install Boost
|
||||
run: |
|
||||
if [ ${{matrix.flag}} != 'no_boost' ]; then
|
||||
if [ ${{runner.os}} == 'Linux' ]; then
|
||||
sudo apt-get -y install libboost-all-dev
|
||||
elif [ ${{runner.os}} == 'macOS' ]; then
|
||||
brew install boost
|
||||
fi
|
||||
fi
|
||||
|
||||
- name: Set Allow Deprecated Flag
|
||||
if: matrix.flag == 'deprecated'
|
||||
run: |
|
||||
|
|
@ -181,7 +186,6 @@ jobs:
|
|||
with:
|
||||
swap-size-gb: 12
|
||||
|
||||
|
||||
- name: Build & Test
|
||||
run: |
|
||||
bash .github/scripts/unix.sh -t
|
||||
|
|
|
|||
|
|
@ -3,9 +3,9 @@
|
|||
To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL. However, there are several intricacies that make this more difficult than it sounds. In general, if you follow the following three rules, GTSAM_EXPORT should work properly. The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules.
|
||||
|
||||
## Usage Rules
|
||||
1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file.
|
||||
1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is defined in a .cpp file, not just a .h file.
|
||||
2. Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if:
|
||||
* At least one of the functions inside that class is declared in a .cpp file and not just the .h file.
|
||||
* At least one of the functions inside that class is defined in a .cpp file and not just the .h file.
|
||||
* You can `GTSAM_EXPORT` any class it inherits from as well. (Note that this implictly requires the class does not derive from a "header-only" class. Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!)
|
||||
3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
|
||||
4. For template specializations, you need to add `GTSAM_EXPORT` to each individual specialization.
|
||||
|
|
@ -28,7 +28,7 @@ But first, we need to understand exactly what `GTSAM_EXPORT` is. `GTSAM_EXPORT`
|
|||
|
||||
Rule #1 doesn't seem very bad, until you combine it with rule #2
|
||||
|
||||
***Compiler Rule #2*** Anything declared in a header file is not included in a DLL.
|
||||
***Compiler Rule #2*** Anything defined in a header file is not included in a DLL.
|
||||
|
||||
When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition. If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL. Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class.
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@ project(gtsam LANGUAGES CXX)
|
|||
set (gtsam_subdirs
|
||||
base
|
||||
basis
|
||||
constrained
|
||||
geometry
|
||||
inference
|
||||
symbolic
|
||||
|
|
|
|||
|
|
@ -48,7 +48,8 @@
|
|||
*/
|
||||
#ifdef __GNUC__
|
||||
#if __GNUC__ >= 7 && __cplusplus >= 201703L
|
||||
namespace boost { namespace serialization { struct U; } }
|
||||
// Based on https://github.com/borglab/gtsam/issues/1738, we define U as a complete type.
|
||||
namespace boost { namespace serialization { struct U{}; } }
|
||||
namespace std { template<> struct is_trivially_default_constructible<boost::serialization::U> : std::false_type {}; }
|
||||
namespace std { template<> struct is_trivially_copy_constructible<boost::serialization::U> : std::false_type {}; }
|
||||
namespace std { template<> struct is_trivially_move_constructible<boost::serialization::U> : std::false_type {}; }
|
||||
|
|
|
|||
|
|
@ -60,8 +60,6 @@ public:
|
|||
TestOptionalStruct() = default;
|
||||
TestOptionalStruct(const int& opt)
|
||||
: opt(opt) {}
|
||||
// A copy constructor is needed for serialization
|
||||
TestOptionalStruct(const TestOptionalStruct& other) = default;
|
||||
bool operator==(const TestOptionalStruct& other) const {
|
||||
// check the values are equal
|
||||
return *opt == *other.opt;
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ void TimingOutline::print(const std::string& outline) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void TimingOutline::printCsvHeader(bool addLineBreak) const {
|
||||
#ifdef GTSAM_USE_BOOST_FEATURES
|
||||
#if GTSAM_USE_BOOST_FEATURES
|
||||
// Order is (CPU time, number of times, wall time, time + children in seconds,
|
||||
// min time, max time)
|
||||
std::cout << label_ + " cpu time (s)" << "," << label_ + " #calls" << ","
|
||||
|
|
@ -134,7 +134,7 @@ void TimingOutline::printCsvHeader(bool addLineBreak) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void TimingOutline::printCsv(bool addLineBreak) const {
|
||||
#ifdef GTSAM_USE_BOOST_FEATURES
|
||||
#if GTSAM_USE_BOOST_FEATURES
|
||||
// Order is (CPU time, number of times, wall time, time + children in seconds,
|
||||
// min time, max time)
|
||||
std::cout << self() << "," << n_ << "," << wall() << "," << secs() << ","
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
# Install headers
|
||||
file(GLOB constraint_headers "*.h")
|
||||
install(FILES ${constraint_headers} DESTINATION include/gtsam/constrained)
|
||||
|
||||
# Build tests
|
||||
add_subdirectory(tests)
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 InequalityPenaltyFunction.cpp
|
||||
* @brief Ramp function to compute inequality constraint violations.
|
||||
* @author Yetong Zhang
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#include <gtsam/constrained/InequalityPenaltyFunction.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function()
|
||||
const {
|
||||
return [this](const double& x, OptionalJacobian<1, 1> H = {}) {
|
||||
return this->operator()(x, H);
|
||||
};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) {
|
||||
if (x < 0) {
|
||||
if (H) {
|
||||
H->setConstant(0);
|
||||
}
|
||||
return 0;
|
||||
} else {
|
||||
if (H) {
|
||||
H->setConstant(1);
|
||||
}
|
||||
return x;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double SmoothRampPoly2::operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H) const {
|
||||
if (x <= 0) {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
}
|
||||
return 0;
|
||||
} else if (x < epsilon_) {
|
||||
if (H) {
|
||||
H->setConstant(2 * a_ * x);
|
||||
}
|
||||
return a_ * pow(x, 2);
|
||||
} else {
|
||||
if (H) {
|
||||
H->setOnes();
|
||||
}
|
||||
return x - epsilon_ / 2;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double SmoothRampPoly3::operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H) const {
|
||||
if (x <= 0) {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
}
|
||||
return 0;
|
||||
} else if (x < epsilon_) {
|
||||
if (H) {
|
||||
H->setConstant(3 * a_ * pow(x, 2) + 2 * b_ * x);
|
||||
}
|
||||
return a_ * pow(x, 3) + b_ * pow(x, 2);
|
||||
} else {
|
||||
if (H) {
|
||||
H->setOnes();
|
||||
}
|
||||
return x;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double SoftPlusFunction::operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H) const {
|
||||
if (H) {
|
||||
H->setConstant(1 / (1 + std::exp(-k_ * x)));
|
||||
}
|
||||
return std::log(1 + std::exp(k_ * x)) / k_;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,149 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 InequalityPenaltyFunction.h
|
||||
* @brief Ramp function to compute inequality constraint violations.
|
||||
* @author Yetong Zhang
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Base class for smooth approximation of the ramp function. */
|
||||
class GTSAM_EXPORT InequalityPenaltyFunction {
|
||||
public:
|
||||
typedef std::shared_ptr<InequalityPenaltyFunction> shared_ptr;
|
||||
typedef std::function<double(const double& x, OptionalJacobian<1, 1> H)>
|
||||
UnaryScalarFunc;
|
||||
|
||||
/** Constructor. */
|
||||
InequalityPenaltyFunction() {}
|
||||
|
||||
/** Destructor. */
|
||||
virtual ~InequalityPenaltyFunction() {}
|
||||
|
||||
virtual double operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H = {}) const = 0;
|
||||
|
||||
virtual UnaryScalarFunc function() const;
|
||||
};
|
||||
|
||||
/** Ramp function f : R -> R.
|
||||
* f(x) = 0 for x <= 0
|
||||
* x for x > 0
|
||||
*/
|
||||
class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction {
|
||||
public:
|
||||
typedef InequalityPenaltyFunction Base;
|
||||
typedef RampFunction This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
public:
|
||||
RampFunction() : Base() {}
|
||||
|
||||
virtual double operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H = {}) const override {
|
||||
return Ramp(x, H);
|
||||
}
|
||||
|
||||
virtual UnaryScalarFunc function() const override { return Ramp; }
|
||||
|
||||
static double Ramp(const double x, OptionalJacobian<1, 1> H = {});
|
||||
};
|
||||
|
||||
/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon].
|
||||
* The coefficients are computed as
|
||||
* a = 1 / (2 * epsilon)
|
||||
* Function f(x) = 0 for x <= 0
|
||||
* a * x^2 for 0 < x < epsilon
|
||||
* x - epsilon/2 for x >= epsilon
|
||||
*/
|
||||
class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction {
|
||||
public:
|
||||
typedef InequalityPenaltyFunction Base;
|
||||
typedef SmoothRampPoly2 This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
double epsilon_;
|
||||
double a_;
|
||||
|
||||
public:
|
||||
/** Constructor.
|
||||
* @param epsilon parameter for adjusting the smoothness of the function.
|
||||
*/
|
||||
SmoothRampPoly2(const double epsilon = 1)
|
||||
: Base(), epsilon_(epsilon), a_(0.5 / epsilon) {}
|
||||
|
||||
virtual double operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H = {}) const override;
|
||||
};
|
||||
|
||||
/** Ramp function approximated with a polynomial of degree 3 in [0, epsilon].
|
||||
* The coefficients are computed as
|
||||
* a = -1 / epsilon^2
|
||||
* b = 2 / epsilon
|
||||
* Function f(x) = 0 for x <= 0
|
||||
* a * x^3 + b * x^2 for 0 < x < epsilon
|
||||
* x for x >= epsilon
|
||||
*/
|
||||
class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction {
|
||||
public:
|
||||
typedef InequalityPenaltyFunction Base;
|
||||
typedef SmoothRampPoly3 This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
double epsilon_;
|
||||
double a_;
|
||||
double b_;
|
||||
|
||||
public:
|
||||
/** Constructor.
|
||||
* @param epsilon parameter for adjusting the smoothness of the function.
|
||||
*/
|
||||
SmoothRampPoly3(const double epsilon = 1)
|
||||
: Base(),
|
||||
epsilon_(epsilon),
|
||||
a_(-1 / (epsilon * epsilon)),
|
||||
b_(2 / epsilon) {}
|
||||
|
||||
virtual double operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H = {}) const override;
|
||||
};
|
||||
|
||||
/** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */
|
||||
class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction {
|
||||
public:
|
||||
typedef InequalityPenaltyFunction Base;
|
||||
typedef SoftPlusFunction This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
double k_;
|
||||
|
||||
public:
|
||||
/** Constructor.
|
||||
* @param k parameter for adjusting the smoothness of the function.
|
||||
*/
|
||||
SoftPlusFunction(const double k = 1) : Base(), k_(k) {}
|
||||
|
||||
virtual double operator()(const double& x,
|
||||
OptionalJacobian<1, 1> H = {}) const override;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 NonlinearConstraint.h
|
||||
* @brief Nonlinear constraints in constrained optimization.
|
||||
* @author Yetong Zhang, Frank Dellaert
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
#include <boost/serialization/base_object.hpp>
|
||||
#endif
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Base class for nonlinear constraint.
|
||||
* The constraint is represented as a NoiseModelFactor with Constrained noise model.
|
||||
* whitenedError() returns The constraint violation vector.
|
||||
* unwhitenedError() returns the sigma-scaled constraint violation vector.
|
||||
*/
|
||||
class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor {
|
||||
public:
|
||||
typedef NoiseModelFactor Base;
|
||||
|
||||
/** Use constructors of NoiseModelFactor. */
|
||||
using Base::Base;
|
||||
|
||||
using NonlinearFactor::error;
|
||||
|
||||
/** Destructor. */
|
||||
virtual ~NonlinearConstraint() {}
|
||||
|
||||
/** Create a cost factor representing the L2 penalty function with scaling coefficient mu. */
|
||||
virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const {
|
||||
return cloneWithNewNoiseModel(penaltyNoise(mu));
|
||||
}
|
||||
|
||||
/** Return the norm of the constraint violation vector. */
|
||||
virtual double violation(const Values& x) const { return sqrt(2 * error(x)); }
|
||||
|
||||
/** Check if constraint violation is within tolerance. */
|
||||
virtual bool feasible(const Values& x, const double tolerance = 1e-5) const {
|
||||
return violation(x) <= tolerance;
|
||||
}
|
||||
|
||||
const Vector sigmas() const { return noiseModel()->sigmas(); }
|
||||
|
||||
// return the hessian of unwhitened error function in each dimension.
|
||||
virtual std::vector<Matrix> unwhitenedHessian(const Values& x) const {
|
||||
throw std::runtime_error("hessian not implemented");
|
||||
}
|
||||
|
||||
protected:
|
||||
/** Noise model used for the penalty function. */
|
||||
SharedDiagonal penaltyNoise(const double mu) const {
|
||||
return noiseModel::Diagonal::Sigmas(noiseModel()->sigmas() / sqrt(mu));
|
||||
}
|
||||
|
||||
/** Default constrained noisemodel used for construction of constraint. */
|
||||
static SharedNoiseModel constrainedNoise(const Vector& sigmas) {
|
||||
return noiseModel::Constrained::MixedSigmas(1.0, sigmas);
|
||||
}
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("NonlinearConstraint",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 NonlinearEqualityConstraint-inl.h
|
||||
* @brief Nonlinear equality constraints in constrained optimization.
|
||||
* @author Yetong Zhang, Frank Dellaert
|
||||
* @date Aug 3, 2024 */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
template <typename T>
|
||||
ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T>& expression,
|
||||
const T& rhs,
|
||||
const Vector& sigmas)
|
||||
: Base(constrainedNoise(sigmas), expression.keysAndDims().first),
|
||||
expression_(expression),
|
||||
rhs_(rhs),
|
||||
dims_(expression.keysAndDims().second) {}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
template <typename T>
|
||||
Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x,
|
||||
OptionalMatrixVecType H) const {
|
||||
// Copy-paste from ExpressionFactor.
|
||||
if (H) {
|
||||
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
|
||||
return -traits<T>::Local(value, rhs_);
|
||||
} else {
|
||||
const T value = expression_.value(x);
|
||||
return -traits<T>::Local(value, rhs_);
|
||||
}
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
template <typename T>
|
||||
NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(const double mu) const {
|
||||
return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_, expression_);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 NonlinearEqualityConstraint.cpp
|
||||
* @brief Nonlinear equality constraints in constrained optimization.
|
||||
* @author Yetong Zhang, Frank Dellaert
|
||||
* @date Aug 3, 2024 */
|
||||
|
||||
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor)
|
||||
: Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const {
|
||||
return factor_->unwhitenedError(x, H);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const {
|
||||
return factor_->cloneWithNewNoiseModel(penaltyNoise(mu));
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph(
|
||||
const NonlinearFactorGraph& graph) {
|
||||
NonlinearEqualityConstraints constraints;
|
||||
for (const auto& factor : graph) {
|
||||
auto noise_factor = std::dynamic_pointer_cast<NoiseModelFactor>(factor);
|
||||
constraints.emplace_shared<ZeroCostConstraint>(noise_factor);
|
||||
}
|
||||
return constraints;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
size_t NonlinearEqualityConstraints::dim() const {
|
||||
size_t dimension = 0;
|
||||
for (const auto& constraint : *this) {
|
||||
dimension += constraint->dim();
|
||||
}
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const {
|
||||
Vector violation(dim());
|
||||
size_t start_idx = 0;
|
||||
for (const auto& constraint : *this) {
|
||||
size_t dim = constraint->dim();
|
||||
violation.middleCols(start_idx, dim) =
|
||||
whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
|
||||
start_idx += dim;
|
||||
}
|
||||
return violation;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
double NonlinearEqualityConstraints::violationNorm(const Values& values) const {
|
||||
return violationVector(values, true).norm();
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const {
|
||||
NonlinearFactorGraph graph;
|
||||
for (const auto& constraint : *this) {
|
||||
graph.add(constraint->penaltyFactor(mu));
|
||||
}
|
||||
return graph;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,182 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 NonlinearEqualityConstraint.h
|
||||
* @brief Nonlinear equality constraints in constrained optimization.
|
||||
* @author Yetong Zhang, Frank Dellaert
|
||||
* @date Aug 3, 2024 */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/constrained/NonlinearConstraint.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Equality constraint base class.
|
||||
*/
|
||||
class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint {
|
||||
public:
|
||||
typedef NonlinearConstraint Base;
|
||||
typedef NonlinearEqualityConstraint This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
/** Default constructor. */
|
||||
using Base::Base;
|
||||
|
||||
/** Destructor. */
|
||||
virtual ~NonlinearEqualityConstraint() {}
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("NonlinearEqualityConstraint",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
/** Equality constraint that force g(x) = M. */
|
||||
template <typename T>
|
||||
class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
|
||||
public:
|
||||
typedef NonlinearEqualityConstraint Base;
|
||||
typedef ExpressionEqualityConstraint This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
Expression<T> expression_;
|
||||
T rhs_;
|
||||
FastVector<int> dims_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor.
|
||||
*
|
||||
* @param expression expression representing g(x).
|
||||
* @param tolerance vector representing tolerance in each dimension.
|
||||
*/
|
||||
ExpressionEqualityConstraint(const Expression<T>& expression, const T& rhs, const Vector& sigmas);
|
||||
|
||||
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
|
||||
|
||||
virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
|
||||
|
||||
const Expression<T>& expression() const { return expression_; }
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("ExpressionEqualityConstraint",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(expression_);
|
||||
ar& BOOST_SERIALIZATION_NVP(rhs_);
|
||||
ar& BOOST_SERIALIZATION_NVP(dims_);
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
/** Equality constraint that enforce the cost factor with zero error.
|
||||
* e.g., for a factor with unwhitened cost 2x-1, the constraint enforces the
|
||||
* equlity 2x-1=0.
|
||||
*/
|
||||
class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint {
|
||||
public:
|
||||
typedef NonlinearEqualityConstraint Base;
|
||||
typedef ZeroCostConstraint This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
NoiseModelFactor::shared_ptr factor_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor.
|
||||
*
|
||||
* @param factor NoiseModel factor.
|
||||
*/
|
||||
ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor);
|
||||
|
||||
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
|
||||
|
||||
virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("ZeroCostConstraint",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(factor_);
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
/// Container of NonlinearEqualityConstraint.
|
||||
class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph<NonlinearEqualityConstraint> {
|
||||
public:
|
||||
typedef std::shared_ptr<NonlinearEqualityConstraints> shared_ptr;
|
||||
typedef FactorGraph<NonlinearEqualityConstraint> Base;
|
||||
|
||||
public:
|
||||
using Base::Base;
|
||||
|
||||
/// Create constraints ensuring the cost of factors of a graph is zero.
|
||||
static NonlinearEqualityConstraints FromCostGraph(const NonlinearFactorGraph& graph);
|
||||
|
||||
size_t dim() const;
|
||||
|
||||
/// Evaluate the constraint violation as a vector
|
||||
Vector violationVector(const Values& values, bool whiten = true) const;
|
||||
|
||||
/// Evaluate the constraint violation (as 2-norm of the violation vector).
|
||||
double violationNorm(const Values& values) const;
|
||||
|
||||
NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("NonlinearEqualityConstraints",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/constrained/NonlinearEqualityConstraint-inl.h>
|
||||
|
|
@ -0,0 +1,179 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 NonlinearInequalityConstraint.cpp
|
||||
* @brief Nonlinear inequality constraints in constrained optimization.
|
||||
* @author Yetong Zhang, Frank Dellaert
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
Vector NonlinearInequalityConstraint::whitenedExpr(const Values& x) const {
|
||||
return noiseModel()->whiten(unwhitenedExpr(x));
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x,
|
||||
OptionalMatrixVecType H) const {
|
||||
Vector error = unwhitenedExpr(x, H);
|
||||
for (size_t i = 0; i < dim(); i++) {
|
||||
if (error(i) < 0) {
|
||||
error(i) = 0;
|
||||
if (H) {
|
||||
for (Matrix& m : *H) {
|
||||
m.row(i).setZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
bool NonlinearInequalityConstraint::active(const Values& x) const {
|
||||
return (unwhitenedExpr(x).array() >= 0).any();
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorCustom(
|
||||
InequalityPenaltyFunction::shared_ptr func, const double mu) const {
|
||||
/// Default behavior, this function should be overriden.
|
||||
return penaltyFactor(mu);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint()
|
||||
const {
|
||||
/// Default behavior, this function should be overriden.
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality(
|
||||
const double mu) const {
|
||||
return createEqualityConstraint()->penaltyFactor(mu);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint(
|
||||
const Double_& expression, const double& sigma)
|
||||
: Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first),
|
||||
expression_(expression),
|
||||
dims_(expression.keysAndDims().second) {}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero(
|
||||
const Double_& expression, const double& sigma) {
|
||||
Double_ neg_expr = Double_(0.0) - expression;
|
||||
return std::make_shared<ScalarExpressionInequalityConstraint>(neg_expr, sigma);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero(
|
||||
const Double_& expression, const double& sigma) {
|
||||
return std::make_shared<ScalarExpressionInequalityConstraint>(expression, sigma);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x,
|
||||
OptionalMatrixVecType H) const {
|
||||
// Copy-paste from ExpressionFactor.
|
||||
if (H) {
|
||||
return Vector1(expression_.valueAndDerivatives(x, keys_, dims_, *H));
|
||||
} else {
|
||||
return Vector1(expression_.value(x));
|
||||
}
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NonlinearEqualityConstraint::shared_ptr
|
||||
ScalarExpressionInequalityConstraint::createEqualityConstraint() const {
|
||||
return std::make_shared<ExpressionEqualityConstraint<double>>(
|
||||
expression_, 0.0, noiseModel()->sigmas());
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor(
|
||||
const double mu) const {
|
||||
Double_ penalty_expression(RampFunction::Ramp, expression_);
|
||||
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, penalty_expression);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorCustom(
|
||||
InequalityPenaltyFunction::shared_ptr func, const double mu) const {
|
||||
if (!func) {
|
||||
return penaltyFactor(mu);
|
||||
}
|
||||
// TODO(yetong): can we pass the functor directly to construct the expression?
|
||||
Double_ error(func->function(), expression_);
|
||||
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, error);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality(
|
||||
const double mu) const {
|
||||
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, expression_);
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
size_t NonlinearInequalityConstraints::dim() const {
|
||||
size_t dimension = 0;
|
||||
for (const auto& constraint : *this) {
|
||||
dimension += constraint->dim();
|
||||
}
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const {
|
||||
Vector violation(dim());
|
||||
size_t start_idx = 0;
|
||||
for (const auto& constraint : *this) {
|
||||
size_t dim = constraint->dim();
|
||||
violation.middleCols(start_idx, dim) =
|
||||
whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
|
||||
start_idx += dim;
|
||||
}
|
||||
return violation;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
double NonlinearInequalityConstraints::violationNorm(const Values& values) const {
|
||||
return violationVector(values, true).norm();
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const {
|
||||
NonlinearFactorGraph graph;
|
||||
for (const auto& constraint : *this) {
|
||||
graph.add(constraint->penaltyFactor(mu));
|
||||
}
|
||||
return graph;
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphCustom(
|
||||
InequalityPenaltyFunction::shared_ptr func, const double mu) const {
|
||||
NonlinearFactorGraph graph;
|
||||
for (const auto& constraint : *this) {
|
||||
graph.add(constraint->penaltyFactorCustom(func, mu));
|
||||
}
|
||||
return graph;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,183 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 NonlinearInequalityConstraint.h
|
||||
* @brief Nonlinear inequality constraints in constrained optimization.
|
||||
* @author Yetong Zhang, Frank Dellaert
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/constrained/InequalityPenaltyFunction.h>
|
||||
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Inequality constraint base class, enforcing g(x) <= 0.
|
||||
*/
|
||||
class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint {
|
||||
public:
|
||||
typedef NonlinearConstraint Base;
|
||||
typedef NonlinearInequalityConstraint This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
/** Default constructor. */
|
||||
using Base::Base;
|
||||
|
||||
/** Destructor. */
|
||||
virtual ~NonlinearInequalityConstraint() {}
|
||||
|
||||
/** Return g(x). */
|
||||
virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
|
||||
|
||||
virtual Vector whitenedExpr(const Values& x) const;
|
||||
|
||||
/** Return ramp(g(x)). */
|
||||
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
|
||||
|
||||
/** Return true if g(x)>=0 in any dimension. */
|
||||
virtual bool active(const Values& x) const override;
|
||||
|
||||
/** Return an equality constraint corresponding to g(x)=0. */
|
||||
virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const;
|
||||
|
||||
/** Cost factor using a customized penalty function. */
|
||||
virtual NoiseModelFactor::shared_ptr penaltyFactorCustom(
|
||||
InequalityPenaltyFunction::shared_ptr func, const double mu = 1.0) const;
|
||||
|
||||
/** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */
|
||||
virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const;
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("NonlinearInequalityConstraint",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
/** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued
|
||||
* nonlinear function.
|
||||
*/
|
||||
class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint {
|
||||
public:
|
||||
typedef NonlinearInequalityConstraint Base;
|
||||
typedef ScalarExpressionInequalityConstraint This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
Double_ expression_;
|
||||
FastVector<int> dims_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor.
|
||||
*
|
||||
* @param expression expression representing g(x) (or -g(x) for GeqZero).
|
||||
* @param sigma scalar representing sigma.
|
||||
*/
|
||||
ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma);
|
||||
|
||||
/** Create an inequality constraint g(x)/sigma >= 0, internally represented as -g(x)/sigma <= 0.
|
||||
*/
|
||||
static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression,
|
||||
const double& sigma);
|
||||
|
||||
/** Create an inequality constraint g(x)/sigma <= 0. */
|
||||
static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression,
|
||||
const double& sigma);
|
||||
|
||||
/** Compute g(x), or -g(x) for objects constructed from GeqZero. */
|
||||
virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override;
|
||||
|
||||
/** Equality constraint representing g(x)/sigma = 0. */
|
||||
NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override;
|
||||
|
||||
/** Penalty function 0.5*mu*||ramp(g(x)/sigma||^2. */
|
||||
NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
|
||||
|
||||
/** Penalty function using a smooth approxiamtion of the ramp funciton. */
|
||||
NoiseModelFactor::shared_ptr penaltyFactorCustom(InequalityPenaltyFunction::shared_ptr func,
|
||||
const double mu = 1.0) const override;
|
||||
|
||||
/** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */
|
||||
virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override;
|
||||
|
||||
/** Return expression g(x), or -g(x) for objects constructed from GeqZero. */
|
||||
const Double_& expression() const { return expression_; }
|
||||
|
||||
/** return a deep copy of this factor. */
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("ExpressionEqualityConstraint",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(expression_);
|
||||
ar& BOOST_SERIALIZATION_NVP(dims_);
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
/// Container of NonlinearInequalityConstraint.
|
||||
class GTSAM_EXPORT NonlinearInequalityConstraints
|
||||
: public FactorGraph<NonlinearInequalityConstraint> {
|
||||
public:
|
||||
typedef FactorGraph<NonlinearInequalityConstraint> Base;
|
||||
typedef NonlinearInequalityConstraints This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
using Base::Base;
|
||||
|
||||
/** Return the total dimension of constraints. */
|
||||
size_t dim() const;
|
||||
|
||||
/** Evaluate the constraint violation as a vector. */
|
||||
Vector violationVector(const Values& values, bool whiten = true) const;
|
||||
|
||||
/** Evaluate the constraint violation (as L2 norm). */
|
||||
double violationNorm(const Values& values) const;
|
||||
|
||||
/** Return the penalty function corresponding to \sum_i||ramp(g_i(x))||^2. */
|
||||
NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
|
||||
|
||||
/** Return the cost graph constructed using a customized penalty function. */
|
||||
NonlinearFactorGraph penaltyGraphCustom(InequalityPenaltyFunction::shared_ptr func,
|
||||
const double mu = 1.0) const;
|
||||
|
||||
private:
|
||||
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp("NonlinearInequalityConstraints",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
# if GTSAM_ENABLE_BOOST_SERIALIZATION is OFF then exclude some tests
|
||||
if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
|
||||
# create a semicolon seperated list of files to exclude
|
||||
set(EXCLUDE_TESTS "testSerializationConstraint.cpp")
|
||||
else()
|
||||
set(EXCLUDE_TESTS "")
|
||||
endif()
|
||||
|
||||
gtsamAddTestsGlob(constraint "test*.cpp" "${EXCLUDE_TESTS}" "gtsam")
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 constrainedExample.h
|
||||
* @brief Simple constrained optimization scenarios.
|
||||
* @author Yetong Zhang
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
// #include <gtsam/constrained/ConstrainedOptProblem.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
|
||||
|
||||
namespace constrained_example {
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/// Exponential function e^x.
|
||||
inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) {
|
||||
double result = exp(x);
|
||||
if (H1) H1->setConstant(result);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Exponential expression e^x.
|
||||
inline Double_ exp(const Double_& x) { return Double_(exp_func, x); }
|
||||
|
||||
/// Pow functor used for pow function.
|
||||
class PowFunctor {
|
||||
private:
|
||||
double c_;
|
||||
|
||||
public:
|
||||
PowFunctor(const double& c) : c_(c) {}
|
||||
|
||||
double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const {
|
||||
if (H1) H1->setConstant(c_ * pow(x, c_ - 1));
|
||||
return pow(x, c_);
|
||||
}
|
||||
};
|
||||
|
||||
/// Pow function.
|
||||
inline Double_ pow(const Double_& x, const double& c) {
|
||||
PowFunctor pow_functor(c);
|
||||
return Double_(pow_functor, x);
|
||||
}
|
||||
|
||||
/// Plus between Double expression and double.
|
||||
inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); }
|
||||
|
||||
/// Negative sign operator.
|
||||
inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; }
|
||||
|
||||
/// Keys for creating expressions.
|
||||
Symbol x1_key('x', 1);
|
||||
Symbol x2_key('x', 2);
|
||||
Double_ x1(x1_key), x2(x2_key);
|
||||
|
||||
} // namespace constrained_example
|
||||
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testInequalityPenaltyFunction.h
|
||||
* @brief unit tests for ramp functions
|
||||
* @author Yetong Zhang
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/constrained/InequalityPenaltyFunction.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
TEST(RampFunction, error_and_jacobian) {
|
||||
/// Helper function for numerical Jacobian computation.
|
||||
auto ramp_helper = [&](const double& x) { return RampFunction::Ramp(x); };
|
||||
|
||||
/// Create a set of values to test the function.
|
||||
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
|
||||
static std::vector<double> expected_r_vec{0.0, 0.0, 1.0, 2.0, 3.0};
|
||||
|
||||
for (size_t i = 0; i < x_vec.size(); i++) {
|
||||
double x = x_vec.at(i);
|
||||
Matrix H;
|
||||
double r = RampFunction::Ramp(x, H);
|
||||
|
||||
/// Check function evaluation.
|
||||
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
|
||||
|
||||
/// Check derivative.
|
||||
if (abs(x) > 1e-6) { // function is not smooth at 0, so Jacobian is undefined.
|
||||
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
|
||||
EXPECT(assert_equal(expected_H, H));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
TEST(RampFunctionPoly2, error_and_jacobian) {
|
||||
/// Helper function for numerical Jacobian computation.
|
||||
SmoothRampPoly2 p_ramp(2.0);
|
||||
auto ramp_helper = [&](const double& x) { return p_ramp(x); };
|
||||
|
||||
/// Create a set of values to test the function.
|
||||
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
|
||||
static std::vector<double> expected_r_vec{0.0, 0.0, 0.25, 1.0, 2.0};
|
||||
|
||||
for (size_t i = 0; i < x_vec.size(); i++) {
|
||||
double x = x_vec.at(i);
|
||||
Matrix H;
|
||||
double r = p_ramp(x, H);
|
||||
|
||||
/// Check function evaluation.
|
||||
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
|
||||
|
||||
/// Check derivative.
|
||||
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
|
||||
EXPECT(assert_equal(expected_H, H, 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
TEST(RampFunctionPoly3, error_and_jacobian) {
|
||||
/// Helper function for numerical Jacobian computation.
|
||||
SmoothRampPoly3 p_ramp(2.0);
|
||||
auto ramp_helper = [&](const double& x) { return p_ramp(x); };
|
||||
|
||||
/// Create a set of values to test the function.
|
||||
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
|
||||
static std::vector<double> expected_r_vec{0.0, 0.0, 0.75, 2.0, 3.0};
|
||||
|
||||
for (size_t i = 0; i < x_vec.size(); i++) {
|
||||
double x = x_vec.at(i);
|
||||
Matrix H;
|
||||
double r = p_ramp(x, H);
|
||||
|
||||
/// Check function evaluation.
|
||||
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
|
||||
|
||||
/// Check derivative.
|
||||
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
|
||||
EXPECT(assert_equal(expected_H, H, 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
TEST(SoftPlusFunction, error_and_jacobian) {
|
||||
/// Helper function for numerical Jacobian computation.
|
||||
SoftPlusFunction soft_plus(0.5);
|
||||
auto soft_plus_helper = [&](const double& x) { return soft_plus(x); };
|
||||
|
||||
/// Create a set of values to test the function.
|
||||
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
|
||||
static std::vector<double> expected_r_vec{
|
||||
0.40282656, 1.38629436, 1.94815397, 2.62652338, 3.40282656};
|
||||
|
||||
for (size_t i = 0; i < x_vec.size(); i++) {
|
||||
double x = x_vec.at(i);
|
||||
Matrix H;
|
||||
double r = soft_plus(x, H);
|
||||
|
||||
/// Check function evaluation.
|
||||
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-6);
|
||||
|
||||
/// Check derivative.
|
||||
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(soft_plus_helper, x, 1e-6);
|
||||
EXPECT(assert_equal(expected_H, H, 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
|
@ -0,0 +1,259 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testNonlinearEqualityConstraint.cpp
|
||||
* @brief unit tests for nonlinear equality constraints
|
||||
* @author Yetong Zhang
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/Test.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include "constrainedExample.h"
|
||||
|
||||
using namespace gtsam;
|
||||
using constrained_example::pow;
|
||||
using constrained_example::x1, constrained_example::x2;
|
||||
using constrained_example::x1_key, constrained_example::x2_key;
|
||||
|
||||
// Test methods of DoubleExpressionEquality.
|
||||
TEST(ExpressionEqualityConstraint, double) {
|
||||
// create constraint from double expression
|
||||
// g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides
|
||||
Vector sigmas = Vector1(0.1);
|
||||
auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);
|
||||
auto constraint = ExpressionEqualityConstraint<double>(g, 0.0, sigmas);
|
||||
|
||||
EXPECT(constraint.noiseModel()->isConstrained());
|
||||
EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
|
||||
|
||||
// Create 2 sets of values for testing.
|
||||
Values values1, values2;
|
||||
values1.insert(x1_key, 0.0);
|
||||
values1.insert(x2_key, 0.0);
|
||||
values2.insert(x1_key, 1.0);
|
||||
values2.insert(x2_key, 1.0);
|
||||
|
||||
// Check that values1 are feasible.
|
||||
EXPECT(constraint.feasible(values1));
|
||||
|
||||
// Check that violation evaluates as 0 at values1.
|
||||
EXPECT(assert_equal(Vector::Zero(1), constraint.unwhitenedError(values1)));
|
||||
EXPECT(assert_equal(Vector::Zero(1), constraint.whitenedError(values1)));
|
||||
EXPECT(assert_equal(0.0, constraint.error(values1)));
|
||||
|
||||
// Check that values2 are indeed deemed infeasible.
|
||||
EXPECT(!constraint.feasible(values2));
|
||||
|
||||
// Check constraint violation is indeed g(x) at values2.
|
||||
EXPECT(assert_equal(Vector::Constant(1, 4.0), constraint.unwhitenedError(values2)));
|
||||
EXPECT(assert_equal(Vector::Constant(1, 40), constraint.whitenedError(values2)));
|
||||
EXPECT(assert_equal(800, constraint.error(values2)));
|
||||
|
||||
// Check dimension is 1 for scalar g.
|
||||
EXPECT(constraint.dim() == 1);
|
||||
|
||||
// Check keys include x1, x2.
|
||||
EXPECT(constraint.keys().size() == 2);
|
||||
EXPECT(x1_key == *constraint.keys().begin());
|
||||
EXPECT(x2_key == *constraint.keys().rbegin());
|
||||
|
||||
// Generate factor representing the term in merit function.
|
||||
double mu = 4;
|
||||
auto merit_factor = constraint.penaltyFactor(mu);
|
||||
|
||||
// Check that noise model sigma == sigmas/sqrt(mu).
|
||||
auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
|
||||
EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
|
||||
|
||||
// Check that error is equal to 0.5*mu * (g(x)+bias)^2/sigmas^2.
|
||||
double expected_error1 = 0; // 0.5 * 4 * ||0||_(0.1^2)^2
|
||||
EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
|
||||
double expected_error2 = 3200; // 0.5 * 4 * ||4||_(0.1^2)^2
|
||||
EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
|
||||
|
||||
// Check jacobian computation is correct.
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
|
||||
}
|
||||
|
||||
// Test methods of VectorExpressionEquality.
|
||||
TEST(ExpressionEqualityConstraint, Vector2) {
|
||||
// g(v1, v2) = v1 + v2, our own example.
|
||||
Vector2_ x1_vec_expr(x1_key);
|
||||
Vector2_ x2_vec_expr(x2_key);
|
||||
auto g = x1_vec_expr + x2_vec_expr;
|
||||
auto sigmas = Vector2(0.1, 0.5);
|
||||
auto constraint = ExpressionEqualityConstraint<Vector2>(g, Vector2::Zero(), sigmas);
|
||||
|
||||
EXPECT(constraint.noiseModel()->isConstrained());
|
||||
EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
|
||||
|
||||
// Create 2 sets of values for testing.
|
||||
Values values1, values2;
|
||||
values1.insert(x1_key, Vector2(1, 1));
|
||||
values1.insert(x2_key, Vector2(-1, -1));
|
||||
values2.insert(x1_key, Vector2(1, 1));
|
||||
values2.insert(x2_key, Vector2(1, 1));
|
||||
|
||||
// Check that values1 are feasible.
|
||||
EXPECT(constraint.feasible(values1));
|
||||
|
||||
// Check that violation evaluates as 0 at values1.
|
||||
auto expected_violation1 = (Vector(2) << 0, 0).finished();
|
||||
EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
|
||||
auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
|
||||
EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
|
||||
|
||||
// Check that values2 are indeed deemed infeasible.
|
||||
EXPECT(!constraint.feasible(values2));
|
||||
|
||||
// Check constraint violation is indeed g(x) at values2.
|
||||
auto expected_violation2 = (Vector(2) << 2, 2).finished();
|
||||
EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
|
||||
|
||||
// Check scaled violation is indeed g(x)/sigmas at values2.
|
||||
auto expected_scaled_violation2 = (Vector(2) << 20, 4).finished();
|
||||
EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
|
||||
|
||||
// Check dim is the dimension of the vector.
|
||||
EXPECT(constraint.dim() == 2);
|
||||
|
||||
// Generate factor representing the term in merit function.
|
||||
double mu = 4;
|
||||
auto merit_factor = constraint.penaltyFactor(mu);
|
||||
|
||||
// Check that noise model sigma == sigmas/sqrt(mu).
|
||||
auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
|
||||
EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
|
||||
|
||||
// Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
|
||||
double expected_error1 = 0; // 0.5 * 4 * ||[1, 0.5]||_([0.1,0.5]^2)^2
|
||||
EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
|
||||
double expected_error2 = 832; // 0.5 * 4 * ||[2, 2]||_([0.1,0.5]^2)^2
|
||||
EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
|
||||
|
||||
// Check jacobian computation is correct.
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
|
||||
}
|
||||
|
||||
// Test methods of FactorZeroErrorConstraint.
|
||||
TEST(ZeroCostConstraint, BetweenFactor) {
|
||||
Key x1_key = 1;
|
||||
Key x2_key = 2;
|
||||
Vector sigmas = Vector2(0.5, 0.1);
|
||||
auto noise = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
|
||||
auto factor = std::make_shared<BetweenFactor<Vector2>>(x1_key, x2_key, Vector2(1, 1), noise);
|
||||
auto constraint = ZeroCostConstraint(factor);
|
||||
|
||||
EXPECT(constraint.noiseModel()->isConstrained());
|
||||
EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
|
||||
|
||||
// Create 2 sets of values for testing.
|
||||
Values values1, values2;
|
||||
values1.insert(x1_key, Vector2(1, 1));
|
||||
values1.insert(x2_key, Vector2(2, 2));
|
||||
values2.insert(x1_key, Vector2(0, 0));
|
||||
values2.insert(x2_key, Vector2(2, 3));
|
||||
|
||||
// Check that values1 are feasible.
|
||||
EXPECT(constraint.feasible(values1));
|
||||
|
||||
// Check that violation evaluates as 0 at values1.
|
||||
auto expected_violation1 = (Vector(2) << 0, 0).finished();
|
||||
EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
|
||||
auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
|
||||
EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
|
||||
|
||||
// Check that values2 are indeed deemed infeasible.
|
||||
EXPECT(!constraint.feasible(values2));
|
||||
|
||||
// Check constraint violation is indeed g(x) at values2.
|
||||
auto expected_violation2 = (Vector(2) << 1, 2).finished();
|
||||
EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
|
||||
|
||||
// Check scaled violation is indeed g(x)/sigmas at values2.
|
||||
auto expected_scaled_violation2 = (Vector(2) << 2, 20).finished();
|
||||
EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
|
||||
|
||||
// Check dim is the dimension of the vector.
|
||||
EXPECT(constraint.dim() == 2);
|
||||
|
||||
// Generate factor representing the term in merit function.
|
||||
double mu = 4;
|
||||
auto merit_factor = constraint.penaltyFactor(mu);
|
||||
|
||||
// Check that noise model sigma == sigmas/sqrt(mu).
|
||||
auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
|
||||
EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
|
||||
|
||||
// Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
|
||||
double expected_error1 = 0; // 0.5 * 4 * ||[0, 0]||_([0.5,0.1]^2)^2
|
||||
EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
|
||||
double expected_error2 = 808; // 0.5 * 4 * ||[1, 2]||_([0.5,0.1]^2)^2
|
||||
EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
|
||||
|
||||
// Check jacobian computation is correct.
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
|
||||
}
|
||||
|
||||
TEST(NonlinearEqualityConstraints, Container) {
|
||||
NonlinearEqualityConstraints constraints;
|
||||
|
||||
Vector sigmas1 = Vector1(10);
|
||||
auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);
|
||||
Vector2_ x1_vec_expr(x1_key);
|
||||
Vector2_ x2_vec_expr(x2_key);
|
||||
auto g2 = x1_vec_expr + x2_vec_expr;
|
||||
Vector sigmas2 = Vector2(0.1, 0.5);
|
||||
|
||||
constraints.emplace_shared<ExpressionEqualityConstraint<double>>(g1, 0.0, sigmas1);
|
||||
constraints.emplace_shared<ExpressionEqualityConstraint<Vector2>>(g2, Vector2::Zero(), sigmas2);
|
||||
|
||||
// Check size.
|
||||
EXPECT_LONGS_EQUAL(2, constraints.size());
|
||||
|
||||
// Check dimension.
|
||||
EXPECT_LONGS_EQUAL(3, constraints.dim());
|
||||
|
||||
// Check keys.
|
||||
KeySet expected_keys;
|
||||
expected_keys.insert(x1_key);
|
||||
expected_keys.insert(x2_key);
|
||||
EXPECT(assert_container_equality(expected_keys, constraints.keys()));
|
||||
|
||||
// Check VariableIndex.
|
||||
VariableIndex vi(constraints);
|
||||
FactorIndices expected_vi_x1{0, 1};
|
||||
FactorIndices expected_vi_x2{0, 1};
|
||||
EXPECT(assert_container_equality(expected_vi_x1, vi[x1_key]));
|
||||
EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key]));
|
||||
|
||||
// Check constraint violation.
|
||||
}
|
||||
|
||||
TEST(NonlinearEqualityConstraints, FromCostGraph) {}
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
|
@ -0,0 +1,117 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testNonlinearInequalityConstraint.h
|
||||
* @brief unit tests for nonlinear inequality constraints
|
||||
* @author Yetong Zhang
|
||||
* @date Aug 3, 2024
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
|
||||
#include "constrainedExample.h"
|
||||
|
||||
using namespace gtsam;
|
||||
using constrained_example::pow;
|
||||
using constrained_example::x1, constrained_example::x2;
|
||||
using constrained_example::x1_key, constrained_example::x2_key;
|
||||
|
||||
// Test methods of DoubleExpressionEquality.
|
||||
TEST(NonlinearInequalityConstraint, ScalarExpressionInequalityConstraint) {
|
||||
// create constraint from double expression
|
||||
// g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides
|
||||
double sigma = 0.1;
|
||||
auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);
|
||||
auto constraint_geq = ScalarExpressionInequalityConstraint::GeqZero(g, sigma);
|
||||
auto constraint_leq = ScalarExpressionInequalityConstraint::LeqZero(g, sigma);
|
||||
|
||||
// Check dimension is 1 for scalar g.
|
||||
EXPECT_LONGS_EQUAL(1, constraint_geq->dim());
|
||||
EXPECT_LONGS_EQUAL(1, constraint_leq->dim());
|
||||
|
||||
// Check keys include x1, x2.
|
||||
KeyVector expected_keys{x1_key, x2_key};
|
||||
EXPECT(assert_container_equality(expected_keys, constraint_leq->keys()));
|
||||
|
||||
// Create 3 sets of values for testing.
|
||||
Values values1, values2, values3;
|
||||
values1.insert(x1_key, -1.0);
|
||||
values1.insert(x2_key, 1.0);
|
||||
values2.insert(x1_key, 1.0);
|
||||
values2.insert(x2_key, 1.0);
|
||||
values3.insert(x1_key, -2.0);
|
||||
values3.insert(x2_key, 1.0);
|
||||
|
||||
// Check that expression evaluation at different values.
|
||||
EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedExpr(values1)));
|
||||
EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedExpr(values2)));
|
||||
EXPECT(assert_equal(Vector1(-8.0), constraint_leq->unwhitenedExpr(values3)));
|
||||
EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedExpr(values1)));
|
||||
EXPECT(assert_equal(Vector1(-4.0), constraint_geq->unwhitenedExpr(values2)));
|
||||
EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedExpr(values3)));
|
||||
|
||||
// Check that constraint violation at different values.
|
||||
EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values1)));
|
||||
EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedError(values2)));
|
||||
EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values3)));
|
||||
EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values1)));
|
||||
EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values2)));
|
||||
EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedError(values3)));
|
||||
|
||||
// Check feasible.
|
||||
EXPECT(constraint_leq->feasible(values1));
|
||||
EXPECT(!constraint_leq->feasible(values2));
|
||||
EXPECT(constraint_leq->feasible(values3));
|
||||
EXPECT(constraint_geq->feasible(values1));
|
||||
EXPECT(constraint_geq->feasible(values2));
|
||||
EXPECT(!constraint_geq->feasible(values3));
|
||||
|
||||
// Check active.
|
||||
EXPECT(constraint_leq->active(values1));
|
||||
EXPECT(constraint_leq->active(values2));
|
||||
EXPECT(!constraint_leq->active(values3));
|
||||
EXPECT(constraint_geq->active(values1));
|
||||
EXPECT(!constraint_geq->active(values2));
|
||||
EXPECT(constraint_geq->active(values3));
|
||||
|
||||
// Check whitenedError of penalty factor.
|
||||
// Expected to be sqrt(mu) / sigma * ramp(g(x))
|
||||
double mu = 9.0;
|
||||
auto penalty_leq = constraint_leq->penaltyFactor(mu);
|
||||
auto penalty_geq = constraint_geq->penaltyFactor(mu);
|
||||
EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values1)));
|
||||
EXPECT(assert_equal(Vector1(120.0), penalty_leq->whitenedError(values2)));
|
||||
EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values3)));
|
||||
EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values1)));
|
||||
EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values2)));
|
||||
EXPECT(assert_equal(Vector1(240.0), penalty_geq->whitenedError(values3)));
|
||||
|
||||
// Check create equality constraint
|
||||
auto constraint_eq1 = constraint_leq->createEqualityConstraint();
|
||||
auto constraint_eq2 = constraint_geq->createEqualityConstraint();
|
||||
EXPECT(assert_equal(0.0, constraint_eq1->error(values1)));
|
||||
EXPECT(assert_equal(800.0, constraint_eq1->error(values2)));
|
||||
EXPECT(assert_equal(3200.0, constraint_eq1->error(values3)));
|
||||
EXPECT(assert_equal(0.0, constraint_eq2->error(values1)));
|
||||
EXPECT(assert_equal(800.0, constraint_eq2->error(values2)));
|
||||
EXPECT(assert_equal(3200.0, constraint_eq2->error(values3)));
|
||||
}
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteDistribution.h>
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/discrete/DiscreteDistribution.h>
|
||||
|
|
|
|||
|
|
@ -183,18 +183,37 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
|
|||
// Get angular velocity omega and translational velocity v from twist xi
|
||||
const Vector3 w = xi.head<3>(), v = xi.tail<3>();
|
||||
|
||||
// Compute rotation using Expmap
|
||||
Matrix3 Jr;
|
||||
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
|
||||
// Instantiate functor for Dexp-related operations:
|
||||
const bool nearZero = (w.dot(w) <= 1e-5);
|
||||
const so3::DexpFunctor local(w, nearZero);
|
||||
|
||||
// Compute translation and optionally its Jacobian Q in w
|
||||
// The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
|
||||
Matrix3 Q;
|
||||
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
|
||||
// Compute rotation using Expmap
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
|
||||
#else
|
||||
const Rot3 R(local.expmap());
|
||||
#endif
|
||||
|
||||
// The translation t = local.leftJacobian() * v.
|
||||
// Here we call applyLeftJacobian, which is faster if you don't need
|
||||
// Jacobians, and returns Jacobian of t with respect to w if asked.
|
||||
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
|
||||
// t_parallel = w * w.dot(v); // translation parallel to axis
|
||||
// w_cross_v = w.cross(v); // translation orthogonal to axis
|
||||
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
|
||||
// but functor does not need R, deals automatically with the case where theta2
|
||||
// is near zero, and also gives us the machinery for the Jacobians.
|
||||
Matrix3 H;
|
||||
const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr);
|
||||
|
||||
if (Hxi) {
|
||||
*Hxi << Jr, Z_3x3, //
|
||||
Q, Jr;
|
||||
// The Jacobian of expmap is given by the right Jacobian of SO(3):
|
||||
const Matrix3 Jr = local.rightJacobian();
|
||||
// We are creating a Pose3, so we still need to chain H with R^T, the
|
||||
// Jacobian of Pose3::Create with respect to t.
|
||||
const Matrix3 Q = R.matrix().transpose() * H;
|
||||
*Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap
|
||||
Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v.
|
||||
}
|
||||
|
||||
return Pose3(R, t);
|
||||
|
|
@ -260,45 +279,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
|
|||
double nearZeroThreshold) {
|
||||
const auto w = xi.head<3>();
|
||||
const auto v = xi.tail<3>();
|
||||
Matrix3 Q;
|
||||
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
|
||||
return Q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
|
||||
// t_parallel = w * w.dot(v); // translation parallel to axis
|
||||
// w_cross_v = w.cross(v); // translation orthogonal to axis
|
||||
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
|
||||
// but functor does not need R, deals automatically with the case where theta2
|
||||
// is near zero, and also gives us the machinery for the Jacobians.
|
||||
Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
|
||||
OptionalJacobian<3, 3> Q,
|
||||
OptionalJacobian<3, 3> J,
|
||||
double nearZeroThreshold) {
|
||||
const double theta2 = w.dot(w);
|
||||
bool nearZero = (theta2 <= nearZeroThreshold);
|
||||
|
||||
// Instantiate functor for Dexp-related operations:
|
||||
bool nearZero = (w.dot(w) <= nearZeroThreshold);
|
||||
so3::DexpFunctor local(w, nearZero);
|
||||
|
||||
// Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
|
||||
// don't need Jacobians, and returns Jacobian of t with respect to w if asked.
|
||||
// Call applyLeftJacobian to get its Jacobian
|
||||
Matrix3 H;
|
||||
Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
|
||||
local.applyLeftJacobian(v, H);
|
||||
|
||||
// We return Jacobians for use in Expmap, so we multiply with X, that
|
||||
// translates from left to right for our right expmap convention:
|
||||
if (Q) {
|
||||
Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
|
||||
*Q = X * H;
|
||||
}
|
||||
|
||||
if (J) {
|
||||
*J = local.rightJacobian(); // = X * local.leftJacobian();
|
||||
}
|
||||
|
||||
return t;
|
||||
// Multiply with R^T to account for the Pose3::Create Jacobian.
|
||||
const Matrix3 R = local.expmap();
|
||||
return R.transpose() * H;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -220,27 +220,10 @@ public:
|
|||
* (see Chirikjian11book2, pg 44, eq 10.95.
|
||||
* The closed-form formula is identical to formula 102 in Barfoot14tro where
|
||||
* Q_l of the SE3 Expmap left derivative matrix is given.
|
||||
* This is the Jacobian of ExpmapTranslation and computed there.
|
||||
*/
|
||||
static Matrix3 ComputeQforExpmapDerivative(
|
||||
const Vector6& xi, double nearZeroThreshold = 1e-5);
|
||||
|
||||
/**
|
||||
* Compute the translation part of the exponential map, with Jacobians.
|
||||
* @param w 3D angular velocity
|
||||
* @param v 3D velocity
|
||||
* @param Q Optionally, compute 3x3 Jacobian wrpt w
|
||||
* @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
|
||||
* @param nearZeroThreshold threshold for small values
|
||||
* @note This function returns Jacobians Q and J corresponding to the bottom
|
||||
* blocks of the SE(3) exponential, and translated from left to right from the
|
||||
* applyLeftJacobian Jacobians.
|
||||
*/
|
||||
static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
|
||||
OptionalJacobian<3, 3> Q = {},
|
||||
OptionalJacobian<3, 3> J = {},
|
||||
double nearZeroThreshold = 1e-5);
|
||||
|
||||
using LieGroup<Pose3, 6>::inverse; // version with derivative
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
|
|||
init(nearZeroApprox);
|
||||
}
|
||||
|
||||
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
|
||||
Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
|
||||
|
||||
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
|
||||
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
|
||||
|
|
@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
|
|||
template <>
|
||||
GTSAM_EXPORT
|
||||
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
|
||||
return so3::ExpmapFunctor(axis, theta).expmap();
|
||||
return SO3(so3::ExpmapFunctor(axis, theta).expmap());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -251,11 +251,11 @@ template <>
|
|||
GTSAM_EXPORT
|
||||
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
|
||||
if (H) {
|
||||
so3::DexpFunctor impl(omega);
|
||||
*H = impl.dexp();
|
||||
return impl.expmap();
|
||||
so3::DexpFunctor local(omega);
|
||||
*H = local.dexp();
|
||||
return SO3(local.expmap());
|
||||
} else {
|
||||
return so3::ExpmapFunctor(omega).expmap();
|
||||
return SO3(so3::ExpmapFunctor(omega).expmap());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor {
|
|||
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
|
||||
|
||||
/// Rodrigues formula
|
||||
SO3 expmap() const;
|
||||
Matrix3 expmap() const;
|
||||
|
||||
protected:
|
||||
void init(bool nearZeroApprox = false);
|
||||
|
|
|
|||
|
|
@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) {
|
|||
|
||||
// axis angle version
|
||||
so3::ExpmapFunctor f1(axis, angle);
|
||||
SO3 actual1 = f1.expmap();
|
||||
SO3 actual1(f1.expmap());
|
||||
CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
|
||||
|
||||
// axis angle version, negative angle
|
||||
so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
|
||||
SO3 actual2 = f2.expmap();
|
||||
SO3 actual2(f2.expmap());
|
||||
CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
|
||||
|
||||
// omega version
|
||||
so3::ExpmapFunctor f3(axis * angle);
|
||||
SO3 actual3 = f3.expmap();
|
||||
SO3 actual3(f3.expmap());
|
||||
CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
|
||||
|
||||
// omega version, negative angle
|
||||
so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
|
||||
SO3 actual4 = f4.expmap();
|
||||
SO3 actual4(f4.expmap());
|
||||
CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
|
||||
}
|
||||
|
||||
|
|
@ -333,7 +333,7 @@ TEST(SO3, CrossB) {
|
|||
Matrix aH1;
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
[nearZero](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZero).crossB(v);
|
||||
};
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
|
|
@ -351,7 +351,7 @@ TEST(SO3, DoubleCrossC) {
|
|||
Matrix aH1;
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
[nearZero](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
|
||||
};
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
|
|
@ -369,7 +369,7 @@ TEST(SO3, ApplyDexp) {
|
|||
Matrix aH1, aH2;
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
[nearZero](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZero).applyDexp(v);
|
||||
};
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
|
|
@ -390,7 +390,7 @@ TEST(SO3, ApplyInvDexp) {
|
|||
Matrix aH1, aH2;
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
[nearZero](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
|
||||
};
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
|
|
@ -412,7 +412,7 @@ TEST(SO3, ApplyLeftJacobian) {
|
|||
Matrix aH1, aH2;
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
[nearZero](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
|
||||
};
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
|
|
@ -433,7 +433,7 @@ TEST(SO3, ApplyLeftJacobianInverse) {
|
|||
Matrix aH1, aH2;
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
[nearZero](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
|
||||
};
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
|
|
|
|||
|
|
@ -196,21 +196,20 @@ namespace gtsam {
|
|||
for (auto&& root: roots_) {
|
||||
std::queue<sharedClique> bfs_queue;
|
||||
|
||||
// first, move the root to the queue
|
||||
bfs_queue.push(root);
|
||||
root = nullptr; // now the root node is owned by the queue
|
||||
// first, steal the root and move it to the queue. This invalidates root
|
||||
bfs_queue.push(std::move(root));
|
||||
|
||||
// do a BFS on the tree, for each node, add its children to the queue, and then delete it from the queue
|
||||
// So if the reference count of the node is 1, it will be deleted, and because its children are in the queue,
|
||||
// the deletion of the node will not trigger a recursive deletion of the children.
|
||||
while (!bfs_queue.empty()) {
|
||||
// move the ownership of the front node from the queue to the current variable
|
||||
auto current = bfs_queue.front();
|
||||
// move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue
|
||||
auto current = std::move(bfs_queue.front());
|
||||
bfs_queue.pop();
|
||||
|
||||
// add the children of the current node to the queue, so that the queue will also own the children nodes.
|
||||
for (auto child: current->children) {
|
||||
bfs_queue.push(child);
|
||||
bfs_queue.push(std::move(child));
|
||||
} // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
|
||||
// the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
|
||||
// deletion of the children.
|
||||
|
|
|
|||
|
|
@ -114,20 +114,20 @@ ClusterTree<GRAPH>::~ClusterTree() {
|
|||
|
||||
for (auto&& root : roots_) {
|
||||
std::queue<sharedNode> bfs_queue;
|
||||
// first, move the root to the queue
|
||||
bfs_queue.push(root);
|
||||
root = nullptr; // now the root node is owned by the queue
|
||||
|
||||
// first, steal the root and move it to the queue. This invalidates root
|
||||
bfs_queue.push(std::move(root));
|
||||
|
||||
// for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue.
|
||||
// so that the recursive deletion will not happen.
|
||||
while (!bfs_queue.empty()) {
|
||||
// move the ownership of the front node from the queue to the current variable
|
||||
auto node = bfs_queue.front();
|
||||
// move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue
|
||||
auto node = std::move(bfs_queue.front());
|
||||
bfs_queue.pop();
|
||||
|
||||
// add the children of the current node to the queue, so that the queue will also own the children nodes.
|
||||
for (auto child : node->children) {
|
||||
bfs_queue.push(child);
|
||||
bfs_queue.push(std::move(child));
|
||||
} // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
|
||||
// the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
|
||||
// deletion of the children.
|
||||
|
|
|
|||
|
|
@ -166,9 +166,9 @@ class ClusterTree {
|
|||
|
||||
/// @}
|
||||
|
||||
protected:
|
||||
~ClusterTree();
|
||||
|
||||
protected:
|
||||
/// @name Details
|
||||
/// @{
|
||||
|
||||
|
|
|
|||
|
|
@ -197,20 +197,19 @@ namespace gtsam {
|
|||
for (auto&& root : roots_) {
|
||||
std::queue<sharedNode> bfs_queue;
|
||||
|
||||
// first, move the root to the queue
|
||||
bfs_queue.push(root);
|
||||
root = nullptr; // now the root node is owned by the queue
|
||||
// first, steal the root and move it to the queue. This invalidates root
|
||||
bfs_queue.push(std::move(root));
|
||||
|
||||
// for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue.
|
||||
// so that the recursive deletion will not happen.
|
||||
while (!bfs_queue.empty()) {
|
||||
// move the ownership of the front node from the queue to the current variable
|
||||
auto node = bfs_queue.front();
|
||||
// move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue
|
||||
auto node = std::move(bfs_queue.front());
|
||||
bfs_queue.pop();
|
||||
|
||||
// add the children of the current node to the queue, so that the queue will also own the children nodes.
|
||||
for (auto&& child : node->children) {
|
||||
bfs_queue.push(child);
|
||||
bfs_queue.push(std::move(child));
|
||||
} // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
|
||||
// the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
|
||||
// deletion of the children.
|
||||
|
|
|
|||
|
|
@ -117,8 +117,10 @@ namespace gtsam {
|
|||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
protected:
|
||||
~EliminationTree();
|
||||
|
||||
public:
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
|
|
|
|||
|
|
@ -64,6 +64,69 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
|
|||
|
||||
return make_pair(nTb, nV);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void GPSFactorArm::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n";
|
||||
cout << " GPS measurement: " << nT_.transpose() << "\n";
|
||||
cout << " Lever arm: " << bL_.transpose() << "\n";
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool GPSFactorArm::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != nullptr && Base::equals(*e, tol) &&
|
||||
traits<Point3>::Equals(nT_, e->nT_, tol) &&
|
||||
traits<Point3>::Equals(bL_, e->bL_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
Vector GPSFactorArm::evaluateError(const Pose3& p,
|
||||
OptionalMatrixType H) const {
|
||||
const Matrix3 nRb = p.rotation().matrix();
|
||||
if (H) {
|
||||
H->resize(3, 6);
|
||||
|
||||
H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
|
||||
H->block<3, 3>(0, 3) = nRb;
|
||||
}
|
||||
|
||||
return p.translation() + nRb * bL_ - nT_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void GPSFactorArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "GPSFactorArmCalib on " << keyFormatter(key()) << "\n";
|
||||
cout << " GPS measurement: " << nT_.transpose() << "\n";
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool GPSFactorArmCalib::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != nullptr && Base::equals(*e, tol) &&
|
||||
traits<Point3>::Equals(nT_, e->nT_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
|
||||
OptionalMatrixType H1, OptionalMatrixType H2) const {
|
||||
const Matrix3 nRb = p.rotation().matrix();
|
||||
if (H1) {
|
||||
H1->resize(3, 6);
|
||||
|
||||
H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
|
||||
H1->block<3, 3>(0, 3) = nRb;
|
||||
}
|
||||
if (H2){
|
||||
H2->resize(3, 3);
|
||||
*H2 = nRb;
|
||||
}
|
||||
|
||||
return p.translation() + nRb * bL - nT_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
|
||||
|
|
@ -85,5 +148,67 @@ Vector GPSFactor2::evaluateError(const NavState& p,
|
|||
}
|
||||
|
||||
//***************************************************************************
|
||||
void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "GPSFactor2Arm on " << keyFormatter(key()) << "\n";
|
||||
cout << " GPS measurement: " << nT_.transpose() << "\n";
|
||||
cout << " Lever arm: " << bL_.transpose() << "\n";
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool GPSFactor2Arm::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != nullptr && Base::equals(*e, tol) &&
|
||||
traits<Point3>::Equals(nT_, e->nT_, tol) &&
|
||||
traits<Point3>::Equals(bL_, e->bL_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
Vector GPSFactor2Arm::evaluateError(const NavState& p,
|
||||
OptionalMatrixType H) const {
|
||||
const Matrix3 nRb = p.attitude().matrix();
|
||||
if (H) {
|
||||
H->resize(3, 9);
|
||||
|
||||
H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
|
||||
H->block<3, 3>(0, 3) = nRb;
|
||||
H->block<3, 3>(0, 6).setZero();
|
||||
}
|
||||
|
||||
return p.position() + nRb * bL_ - nT_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void GPSFactor2ArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "GPSFactor2ArmCalib on " << keyFormatter(key()) << "\n";
|
||||
cout << " GPS measurement: " << nT_.transpose() << "\n";
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool GPSFactor2ArmCalib::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != nullptr && Base::equals(*e, tol) &&
|
||||
traits<Point3>::Equals(nT_, e->nT_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
|
||||
OptionalMatrixType H1, OptionalMatrixType H2) const {
|
||||
const Matrix3 nRb = p.attitude().matrix();
|
||||
if (H1) {
|
||||
H1->resize(3, 9);
|
||||
|
||||
H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
|
||||
H1->block<3, 3>(0, 3) = nRb;
|
||||
H1->block<3, 3>(0, 6).setZero();
|
||||
}
|
||||
if (H2){
|
||||
H2->resize(3, 3);
|
||||
*H2 = nRb;
|
||||
}
|
||||
|
||||
return p.position() + nRb * bL - nT_;
|
||||
}
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* Prior on position in a Cartesian frame.
|
||||
* If there exists a non-zero lever arm between body frame and GPS
|
||||
* antenna, instead use GPSFactorArm.
|
||||
* Possibilities include:
|
||||
* ENU: East-North-Up navigation frame at some local origin
|
||||
* NED: North-East-Down navigation frame at some local origin
|
||||
|
|
@ -38,7 +40,7 @@ private:
|
|||
|
||||
typedef NoiseModelFactorN<Pose3> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -83,6 +85,7 @@ public:
|
|||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
|
@ -112,7 +115,146 @@ private:
|
|||
};
|
||||
|
||||
/**
|
||||
* Version of GPSFactor for NavState
|
||||
* Version of GPSFactor (for Pose3) with lever arm between GPS and Body frame.
|
||||
* Because the actual location of the antenna depends on both position and
|
||||
* attitude, providing a lever arm makes components of the attitude observable
|
||||
* and accounts for position measurement vs state discrepancies while turning.
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
|
||||
|
||||
private:
|
||||
|
||||
typedef NoiseModelFactorN<Pose3> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
|
||||
///< position of the GPS antenna in the body frame
|
||||
|
||||
public:
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef std::shared_ptr<GPSFactorArm> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef GPSFactorArm This;
|
||||
|
||||
/// default constructor - only use for serialization
|
||||
GPSFactorArm():nT_(0, 0, 0), bL_(0, 0, 0) {}
|
||||
|
||||
~GPSFactorArm() override {}
|
||||
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key key of the Pose3 variable related to this measurement
|
||||
* @param gpsIn gps measurement, in Cartesian navigation frame
|
||||
* @param leverArm translation from the body frame origin to the gps antenna, in body frame
|
||||
* @param model Gaussian noise model
|
||||
*/
|
||||
GPSFactorArm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) :
|
||||
Base(model, key), nT_(gpsIn), bL_(leverArm) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
|
||||
|
||||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
||||
/// return the lever arm, a position in the body frame
|
||||
inline const Point3 & leverArm() const {
|
||||
return bL_;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GPSFactorArm (for Pose3) with unknown lever arm between GPS and
|
||||
* Body frame. Because the actual location of the antenna depends on both
|
||||
* position and attitude, providing a lever arm makes components of the attitude
|
||||
* observable and accounts for position measurement vs state discrepancies while
|
||||
* turning.
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT GPSFactorArmCalib: public NoiseModelFactorN<Pose3, Point3> {
|
||||
|
||||
private:
|
||||
|
||||
typedef NoiseModelFactorN<Pose3, Point3> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
|
||||
public:
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef std::shared_ptr<GPSFactorArmCalib> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef GPSFactorArmCalib This;
|
||||
|
||||
/// default constructor - only use for serialization
|
||||
GPSFactorArmCalib() : nT_(0, 0, 0) {}
|
||||
|
||||
~GPSFactorArmCalib() override {}
|
||||
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key1 key of the Pose3 variable related to this measurement
|
||||
* @param key2 key of the Point3 variable related to the lever arm
|
||||
* @param gpsIn gps measurement, in Cartesian navigation frame
|
||||
* @param leverArm translation from the body frame origin to the gps antenna, in body frame
|
||||
* @param model Gaussian noise model
|
||||
*/
|
||||
GPSFactorArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2), nT_(gpsIn) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
|
||||
|
||||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& p, const Point3& bL, OptionalMatrixType H1,
|
||||
OptionalMatrixType H2) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GPSFactor for NavState, assuming zero lever arm between body frame
|
||||
* and GPS. If there exists a non-zero lever arm between body frame and GPS
|
||||
* antenna, instead use GPSFactor2Arm.
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
|
||||
|
|
@ -121,7 +263,7 @@ private:
|
|||
|
||||
typedef NoiseModelFactorN<NavState> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -139,7 +281,11 @@ public:
|
|||
|
||||
~GPSFactor2() override {}
|
||||
|
||||
/// Constructor from a measurement in a Cartesian frame.
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key key of the NavState variable related to this measurement
|
||||
* @param gpsIn gps measurement, in Cartesian navigation frame
|
||||
* @param model Gaussian noise model
|
||||
*/
|
||||
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
|
||||
Base(model, key), nT_(gpsIn) {
|
||||
}
|
||||
|
|
@ -160,6 +306,7 @@ public:
|
|||
/// vector of errors
|
||||
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
|
@ -180,4 +327,140 @@ private:
|
|||
#endif
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GPSFactor2 with lever arm between GPS and Body frame.
|
||||
* Because the actual location of the antenna depends on both position and
|
||||
* attitude, providing a lever arm makes components of the attitude observable
|
||||
* and accounts for position measurement vs state discrepancies while turning.
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {
|
||||
|
||||
private:
|
||||
|
||||
typedef NoiseModelFactorN<NavState> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
|
||||
///< position of the GPS antenna in the body frame
|
||||
|
||||
public:
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef std::shared_ptr<GPSFactor2Arm> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef GPSFactor2Arm This;
|
||||
|
||||
/// default constructor - only use for serialization
|
||||
GPSFactor2Arm():nT_(0, 0, 0), bL_(0, 0, 0) {}
|
||||
|
||||
~GPSFactor2Arm() override {}
|
||||
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key key of the NavState variable related to this measurement
|
||||
* @param gpsIn gps measurement, in Cartesian navigation frame
|
||||
* @param leverArm translation from the body frame origin to the gps antenna, in body frame
|
||||
* @param model noise model for the factor's residual
|
||||
*/
|
||||
GPSFactor2Arm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) :
|
||||
Base(model, key), nT_(gpsIn), bL_(leverArm) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
|
||||
|
||||
/// vector of errors
|
||||
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
||||
/// return the lever arm, a position in the body frame
|
||||
inline const Point3 & leverArm() const {
|
||||
return bL_;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GPSFactor2Arm for an unknown lever arm between GPS and Body frame.
|
||||
* Because the actual location of the antenna depends on both position and
|
||||
* attitude, providing a lever arm makes components of the attitude observable
|
||||
* and accounts for position measurement vs state discrepancies while turning.
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT GPSFactor2ArmCalib: public NoiseModelFactorN<NavState, Point3> {
|
||||
|
||||
private:
|
||||
|
||||
typedef NoiseModelFactorN<NavState, Point3> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
|
||||
public:
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef std::shared_ptr<GPSFactor2ArmCalib> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef GPSFactor2ArmCalib This;
|
||||
|
||||
/// default constructor - only use for serialization
|
||||
GPSFactor2ArmCalib():nT_(0, 0, 0) {}
|
||||
|
||||
~GPSFactor2ArmCalib() override {}
|
||||
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key1 key of the NavState variable related to this measurement
|
||||
* @param key2 key of the Point3 variable related to the lever arm
|
||||
* @param gpsIn gps measurement, in Cartesian navigation frame
|
||||
* @param model Gaussian noise model
|
||||
*/
|
||||
GPSFactor2ArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2), nT_(gpsIn) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
|
||||
|
||||
/// vector of errors
|
||||
Vector evaluateError(const NavState& p, const Point3& bL,
|
||||
OptionalMatrixType H1,
|
||||
OptionalMatrixType H2) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -117,20 +117,32 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
|
|||
// Get angular velocity w and components rho (for t) and nu (for v) from xi
|
||||
Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
|
||||
|
||||
// Compute rotation using Expmap
|
||||
Matrix3 Jr;
|
||||
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
|
||||
// Instantiate functor for Dexp-related operations:
|
||||
const bool nearZero = (w.dot(w) <= 1e-5);
|
||||
const so3::DexpFunctor local(w, nearZero);
|
||||
|
||||
// Compute translations and optionally their Jacobians Q in w
|
||||
// The Jacobians with respect to rho and nu are equal to Jr
|
||||
Matrix3 Qt, Qv;
|
||||
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
|
||||
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
|
||||
// Compute rotation using Expmap
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
|
||||
#else
|
||||
const Rot3 R(local.expmap());
|
||||
#endif
|
||||
|
||||
// Compute translation and velocity. See Pose3::Expmap
|
||||
Matrix3 H_t_w, H_v_w;
|
||||
const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr);
|
||||
const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr);
|
||||
|
||||
if (Hxi) {
|
||||
*Hxi << Jr, Z_3x3, Z_3x3, //
|
||||
Qt, Jr, Z_3x3, //
|
||||
Qv, Z_3x3, Jr;
|
||||
const Matrix3 Jr = local.rightJacobian();
|
||||
// We are creating a NavState, so we still need to chain H_t_w and H_v_w
|
||||
// with R^T, the Jacobian of Navstate::Create with respect to both t and v.
|
||||
const Matrix3 M = R.matrix();
|
||||
*Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap
|
||||
M.transpose() * H_t_w, Jr, Z_3x3, //
|
||||
M.transpose() * H_v_w, Z_3x3, Jr;
|
||||
// In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83).
|
||||
// J_l is the Jacobian of applyLeftJacobian in the second argument.
|
||||
}
|
||||
|
||||
return NavState(R, t, v);
|
||||
|
|
@ -232,10 +244,21 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
|
|||
Vector3 rho = xi.segment<3>(3);
|
||||
Vector3 nu = xi.tail<3>();
|
||||
|
||||
Matrix3 Qt, Qv;
|
||||
const Rot3 R = Rot3::Expmap(w);
|
||||
Pose3::ExpmapTranslation(w, rho, Qt);
|
||||
Pose3::ExpmapTranslation(w, nu, Qv);
|
||||
// Instantiate functor for Dexp-related operations:
|
||||
const bool nearZero = (w.dot(w) <= 1e-5);
|
||||
const so3::DexpFunctor local(w, nearZero);
|
||||
|
||||
// Call applyLeftJacobian to get its Jacobians
|
||||
Matrix3 H_t_w, H_v_w;
|
||||
local.applyLeftJacobian(rho, H_t_w);
|
||||
local.applyLeftJacobian(nu, H_v_w);
|
||||
|
||||
// Multiply with R^T to account for NavState::Create Jacobian.
|
||||
const Matrix3 R = local.expmap();
|
||||
const Matrix3 Qt = R.transpose() * H_t_w;
|
||||
const Matrix3 Qv = R.transpose() * H_v_w;
|
||||
|
||||
// Now compute the blocks of the LogmapDerivative Jacobian
|
||||
const Matrix3 Jw = Rot3::LogmapDerivative(w);
|
||||
const Matrix3 Qt2 = -Jw * Qt * Jw;
|
||||
const Matrix3 Qv2 = -Jw * Qv * Jw;
|
||||
|
|
@ -247,7 +270,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
|
|||
return J;
|
||||
}
|
||||
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
|
||||
ChartJacobian Hxi) {
|
||||
|
|
|
|||
|
|
@ -144,7 +144,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
auto p = testing::Params();
|
||||
testing::SomeMeasurements measurements;
|
||||
|
||||
auto preintegrated = [=](const Vector3& a, const Vector3& w) {
|
||||
auto preintegrated = [&](const Vector3& a, const Vector3& w) {
|
||||
PreintegratedImuMeasurements pim(p, Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
return pim.preintegrated();
|
||||
|
|
|
|||
|
|
@ -44,7 +44,11 @@ const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
|
|||
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
|
||||
|
||||
// Dekalb-Peachtree Airport runway 2L
|
||||
const double lat = 33.87071, lon = -84.30482, h = 274;
|
||||
const double lat = 33.87071, lon = -84.30482, h = 274;\
|
||||
|
||||
// Random lever arm
|
||||
const Point3 leverArm(0.1, 0.2, 0.3);
|
||||
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
|
|
@ -79,6 +83,77 @@ TEST( GPSFactor, Constructor ) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
TEST( GPSFactorArm, Constructor ) {
|
||||
using namespace example;
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double E, N, U;
|
||||
origin_ENU.Forward(lat, lon, h, E, N, U);
|
||||
|
||||
// Factor
|
||||
Key key(1);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
GPSFactorArm factor(key, Point3(E, N, U), leverArm, model);
|
||||
|
||||
// Create a linearization point at zero error
|
||||
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
|
||||
const Point3 np = Point3(E, N, U) - nRb * leverArm;
|
||||
Pose3 T(nRb, np);
|
||||
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
|
||||
[&factor](const Pose3& T) { return factor.evaluateError(T); }, T);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH;
|
||||
factor.evaluateError(T, actualH);
|
||||
|
||||
// Verify we get the expected error
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
TEST( GPSFactorArmCalib, Constructor ) {
|
||||
using namespace example;
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double E, N, U;
|
||||
origin_ENU.Forward(lat, lon, h, E, N, U);
|
||||
|
||||
// Factor
|
||||
Key key1(1), key2(2);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
GPSFactorArmCalib factor(key1, key2, Point3(E, N, U), model);
|
||||
|
||||
// Create a linearization point at zero error
|
||||
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
|
||||
const Point3 np = Point3(E, N, U) - nRb * leverArm;
|
||||
Pose3 T(nRb, np);
|
||||
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
||||
[&factor, &leverArm](const Pose3& pose_arg) {
|
||||
return factor.evaluateError(pose_arg, leverArm);
|
||||
},
|
||||
T);
|
||||
Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
|
||||
[&factor, &T](const Point3& point_arg) {
|
||||
return factor.evaluateError(T, point_arg);
|
||||
},
|
||||
leverArm);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH1, actualH2;
|
||||
factor.evaluateError(T, leverArm, actualH1, actualH2);
|
||||
|
||||
// Verify we get the expected error
|
||||
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
|
||||
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
TEST( GPSFactor2, Constructor ) {
|
||||
using namespace example;
|
||||
|
|
@ -108,6 +183,77 @@ TEST( GPSFactor2, Constructor ) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
TEST( GPSFactor2Arm, Constructor ) {
|
||||
using namespace example;
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double E, N, U;
|
||||
origin_ENU.Forward(lat, lon, h, E, N, U);
|
||||
|
||||
// Factor
|
||||
Key key(1);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
GPSFactor2Arm factor(key, Point3(E, N, U), leverArm, model);
|
||||
|
||||
// Create a linearization point at zero error
|
||||
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
|
||||
const Point3 np = Point3(E, N, U) - nRb * leverArm;
|
||||
NavState T(nRb, np, Vector3::Zero());
|
||||
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH = numericalDerivative11<Vector, NavState>(
|
||||
[&factor](const NavState& T) { return factor.evaluateError(T); }, T);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH;
|
||||
factor.evaluateError(T, actualH);
|
||||
|
||||
// Verify we get the expected error
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
TEST( GPSFactor2ArmCalib, Constructor ) {
|
||||
using namespace example;
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double E, N, U;
|
||||
origin_ENU.Forward(lat, lon, h, E, N, U);
|
||||
|
||||
// Factor
|
||||
Key key1(1), key2(2);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
GPSFactor2ArmCalib factor(key1, key2, Point3(E, N, U), model);
|
||||
|
||||
// Create a linearization point at zero error
|
||||
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
|
||||
const Point3 np = Point3(E, N, U) - nRb * leverArm;
|
||||
NavState T(nRb, np, Vector3::Zero());
|
||||
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH1 = numericalDerivative11<Vector, NavState>(
|
||||
[&factor, &leverArm](const NavState& nav_arg) {
|
||||
return factor.evaluateError(nav_arg, leverArm);
|
||||
},
|
||||
T);
|
||||
Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
|
||||
[&factor, &T](const Point3& point_arg) {
|
||||
return factor.evaluateError(T, point_arg);
|
||||
},
|
||||
leverArm);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH1, actualH2;
|
||||
factor.evaluateError(T, leverArm, actualH1, actualH2);
|
||||
|
||||
// Verify we get the expected error
|
||||
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
|
||||
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
TEST(GPSData, init) {
|
||||
|
||||
|
|
|
|||
|
|
@ -399,7 +399,7 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
|
|||
Vector3 measuredOmega(0.1, 0, 0);
|
||||
double deltaT = 0.5;
|
||||
|
||||
auto evaluateRotation = [=](const Vector3 biasOmega) {
|
||||
auto evaluateRotation = [&measuredOmega, &deltaT](const Vector3 biasOmega) {
|
||||
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
|
||||
};
|
||||
|
||||
|
|
@ -424,7 +424,7 @@ TEST(ImuFactor, PartialDerivativeLogmap) {
|
|||
// Measurements
|
||||
Vector3 deltaTheta(0, 0, 0);
|
||||
|
||||
auto evaluateLogRotation = [=](const Vector3 delta) {
|
||||
auto evaluateLogRotation = [&thetaHat](const Vector3 delta) {
|
||||
return Rot3::Logmap(
|
||||
Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
|
||||
};
|
||||
|
|
|
|||
|
|
@ -43,21 +43,21 @@ TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
|
|||
testing::SomeMeasurements measurements;
|
||||
|
||||
std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
|
||||
[=](const Vector3& a, const Vector3& w) {
|
||||
[&](const Vector3& a, const Vector3& w) {
|
||||
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
return pim.deltaRij();
|
||||
};
|
||||
|
||||
std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
|
||||
[=](const Vector3& a, const Vector3& w) {
|
||||
[&](const Vector3& a, const Vector3& w) {
|
||||
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
return pim.deltaPij();
|
||||
};
|
||||
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
|
||||
[=](const Vector3& a, const Vector3& w) {
|
||||
[&](const Vector3& a, const Vector3& w) {
|
||||
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
return pim.deltaVij();
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@ TEST(ImuFactor, BiasCorrectionJacobians) {
|
|||
testing::SomeMeasurements measurements;
|
||||
|
||||
std::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
|
||||
[=](const Vector3& a, const Vector3& w) {
|
||||
[&](const Vector3& a, const Vector3& w) {
|
||||
TangentPreintegration pim(testing::Params(), Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
return pim.preintegrated();
|
||||
|
|
@ -149,7 +149,7 @@ TEST(TangentPreintegration, Compose) {
|
|||
TEST(TangentPreintegration, MergedBiasDerivatives) {
|
||||
testing::SomeMeasurements measurements;
|
||||
|
||||
auto f = [=](const Vector3& a, const Vector3& w) {
|
||||
auto f = [&](const Vector3& a, const Vector3& w) {
|
||||
TangentPreintegration pim02(testing::Params(), Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim02);
|
||||
testing::integrateMeasurements(measurements, &pim02);
|
||||
|
|
|
|||
|
|
@ -34,6 +34,8 @@ namespace gtsam {
|
|||
// Forward declares
|
||||
class Values;
|
||||
template<typename T> class ExpressionFactor;
|
||||
template<typename T> class ExpressionEqualityConstraint;
|
||||
class ScalarExpressionInequalityConstraint;
|
||||
|
||||
namespace internal {
|
||||
template<typename T> class ExecutionTrace;
|
||||
|
|
@ -206,6 +208,8 @@ protected:
|
|||
// be very selective on who can access these private methods:
|
||||
friend class ExpressionFactor<T> ;
|
||||
friend class internal::ExpressionNode<T>;
|
||||
friend class ExpressionEqualityConstraint<T>;
|
||||
friend class ScalarExpressionInequalityConstraint;
|
||||
|
||||
// and add tests
|
||||
friend class ::ExpressionFactorShallowTest;
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
|
||||
|
|
@ -40,14 +41,11 @@ namespace gtsam {
|
|||
* \nosubgrouping
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality: public NoiseModelFactorN<VALUE> {
|
||||
class NonlinearEquality: public NonlinearEqualityConstraint {
|
||||
|
||||
public:
|
||||
typedef VALUE T;
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using NoiseModelFactor1<VALUE>::evaluateError;
|
||||
|
||||
private:
|
||||
|
||||
// feasible value
|
||||
|
|
@ -63,7 +61,7 @@ private:
|
|||
using This = NonlinearEquality<VALUE>;
|
||||
|
||||
// typedef to base class
|
||||
using Base = NoiseModelFactorN<VALUE>;
|
||||
using Base = NonlinearEqualityConstraint;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -88,7 +86,7 @@ public:
|
|||
const CompareFunction &_compare = std::bind(traits<T>::Equals,
|
||||
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
|
||||
Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
|
||||
j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
|
||||
KeyVector{j}), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
|
||||
compare_(_compare) {
|
||||
}
|
||||
|
||||
|
|
@ -99,10 +97,14 @@ public:
|
|||
const CompareFunction &_compare = std::bind(traits<T>::Equals,
|
||||
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
|
||||
Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
|
||||
j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
|
||||
KeyVector{j}), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
|
||||
compare_(_compare) {
|
||||
}
|
||||
|
||||
Key key() const {
|
||||
return keys().front();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
@ -139,7 +141,7 @@ public:
|
|||
}
|
||||
|
||||
/// Error function
|
||||
Vector evaluateError(const T& xj, OptionalMatrixType H) const override {
|
||||
Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const {
|
||||
const size_t nj = traits<T>::GetDimension(feasible_);
|
||||
if (allow_error_) {
|
||||
if (H)
|
||||
|
|
@ -158,11 +160,20 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
|
||||
VALUE x1 = x.at<VALUE>(key());
|
||||
if (H) {
|
||||
return evaluateError(x1, &(H->front()));
|
||||
} else {
|
||||
return evaluateError(x1);
|
||||
}
|
||||
}
|
||||
|
||||
/// Linearize is over-written, because base linearization tries to whiten
|
||||
GaussianFactor::shared_ptr linearize(const Values& x) const override {
|
||||
const T& xj = x.at<T>(this->key());
|
||||
Matrix A;
|
||||
Vector b = evaluateError(xj, A);
|
||||
Vector b = evaluateError(xj, &A);
|
||||
SharedDiagonal model = noiseModel::Constrained::All(b.size());
|
||||
return GaussianFactor::shared_ptr(
|
||||
new JacobianFactor(this->key(), A, b, model));
|
||||
|
|
@ -206,16 +217,13 @@ struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
|
|||
* Simple unary equality constraint - fixes a value for a variable
|
||||
*/
|
||||
template<class VALUE>
|
||||
class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
|
||||
class NonlinearEquality1: public NonlinearEqualityConstraint {
|
||||
|
||||
public:
|
||||
typedef VALUE X;
|
||||
|
||||
// Provide access to Matrix& version of evaluateError:
|
||||
using NoiseModelFactor1<VALUE>::evaluateError;
|
||||
|
||||
protected:
|
||||
typedef NoiseModelFactorN<VALUE> Base;
|
||||
typedef NonlinearEqualityConstraint Base;
|
||||
typedef NonlinearEquality1<VALUE> This;
|
||||
|
||||
/// Default constructor to allow for serialization
|
||||
|
|
@ -240,7 +248,7 @@ public:
|
|||
NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
|
||||
std::abs(mu)),
|
||||
key),
|
||||
KeyVector{key}),
|
||||
value_(value) {}
|
||||
|
||||
~NonlinearEquality1() override {
|
||||
|
|
@ -252,14 +260,25 @@ public:
|
|||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
Key key() const { return keys().front(); }
|
||||
|
||||
/// g(x) with optional derivative
|
||||
Vector evaluateError(const X& x1, OptionalMatrixType H) const override {
|
||||
Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const {
|
||||
if (H)
|
||||
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return traits<X>::Local(value_,x1);
|
||||
}
|
||||
|
||||
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
|
||||
X x1 = x.at<X>(key());
|
||||
if (H) {
|
||||
return evaluateError(x1, &(H->front()));
|
||||
} else {
|
||||
return evaluateError(x1);
|
||||
}
|
||||
}
|
||||
|
||||
/// Print
|
||||
void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
|
|
@ -298,10 +317,10 @@ struct traits<NonlinearEquality1<VALUE> >
|
|||
* be the same.
|
||||
*/
|
||||
template <class T>
|
||||
class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
|
||||
class NonlinearEquality2 : public NonlinearEqualityConstraint {
|
||||
protected:
|
||||
using Base = NoiseModelFactorN<T, T>;
|
||||
using This = NonlinearEquality2<T>;
|
||||
typedef NonlinearEqualityConstraint Base;
|
||||
typedef NonlinearEquality2<T> This;
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(T)
|
||||
|
||||
|
|
@ -311,9 +330,6 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
|
|||
public:
|
||||
typedef std::shared_ptr<NonlinearEquality2<T>> shared_ptr;
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
|
|
@ -323,7 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
|
|||
*/
|
||||
NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
|
||||
: Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
|
||||
key1, key2) {}
|
||||
KeyVector{key1, key2}) {}
|
||||
~NonlinearEquality2() override {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
|
|
@ -334,13 +350,23 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
|
|||
|
||||
/// g(x) with optional derivative2
|
||||
Vector evaluateError(
|
||||
const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override {
|
||||
const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const {
|
||||
static const size_t p = traits<T>::dimension;
|
||||
if (H1) *H1 = -Matrix::Identity(p, p);
|
||||
if (H2) *H2 = Matrix::Identity(p, p);
|
||||
return traits<T>::Local(x1, x2);
|
||||
}
|
||||
|
||||
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
|
||||
T x1 = x.at<T>(keys().front());
|
||||
T x2 = x.at<T>(keys().back());
|
||||
if (H) {
|
||||
return evaluateError(x1, x2, &(H->front()), &(H->back()));
|
||||
} else {
|
||||
return evaluateError(x1, x2);
|
||||
}
|
||||
}
|
||||
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -281,6 +281,8 @@ public:
|
|||
*/
|
||||
double weight(const Values& c) const;
|
||||
|
||||
using NonlinearFactor::error;
|
||||
|
||||
/**
|
||||
* Calculate the error of the factor.
|
||||
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -30,20 +31,17 @@ namespace gtsam {
|
|||
* @ingroup slam
|
||||
*/
|
||||
template<class VALUE>
|
||||
struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
|
||||
struct BoundingConstraint1: public NonlinearInequalityConstraint {
|
||||
typedef VALUE X;
|
||||
typedef NoiseModelFactorN<VALUE> Base;
|
||||
typedef NonlinearInequalityConstraint Base;
|
||||
typedef std::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
double threshold_;
|
||||
bool isGreaterThan_; /// flag for greater/less than
|
||||
|
||||
BoundingConstraint1(Key key, double threshold,
|
||||
bool isGreaterThan, double mu = 1000.0) :
|
||||
Base(noiseModel::Constrained::All(1, mu), key),
|
||||
Base(noiseModel::Constrained::All(1, mu), KeyVector{key}),
|
||||
threshold_(threshold), isGreaterThan_(isGreaterThan) {
|
||||
}
|
||||
|
||||
|
|
@ -51,6 +49,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
|
|||
|
||||
inline double threshold() const { return threshold_; }
|
||||
inline bool isGreaterThan() const { return isGreaterThan_; }
|
||||
inline Key key() const { return keys().front(); }
|
||||
|
||||
/**
|
||||
* function producing a scalar value to compare to the threshold
|
||||
|
|
@ -60,14 +59,23 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
|
|||
virtual double value(const X& x, OptionalMatrixType H =
|
||||
OptionalNone) const = 0;
|
||||
|
||||
/** active when constraint *NOT* met */
|
||||
bool active(const Values& c) const override {
|
||||
// note: still active at equality to avoid zigzagging
|
||||
double x = value(c.at<X>(this->key()));
|
||||
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
|
||||
Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override {
|
||||
if (H) {
|
||||
double d = value(x.at<X>(this->key()), &(H->front()));
|
||||
if (isGreaterThan_) {
|
||||
H->front() *= -1;
|
||||
return Vector1(threshold_ - d);
|
||||
} else {
|
||||
return Vector1(d - threshold_);
|
||||
}
|
||||
} else {
|
||||
double d = value(x.at<X>(this->key()));
|
||||
return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_);
|
||||
}
|
||||
}
|
||||
|
||||
Vector evaluateError(const X& x, OptionalMatrixType H) const override {
|
||||
/// TODO: This should be deprecated.
|
||||
Vector evaluateError(const X& x, OptionalMatrixType H = {}) const {
|
||||
Matrix D;
|
||||
double error = value(x, &D) - threshold_;
|
||||
if (H) {
|
||||
|
|
@ -102,22 +110,19 @@ private:
|
|||
* to implement for specific systems
|
||||
*/
|
||||
template<class VALUE1, class VALUE2>
|
||||
struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
|
||||
struct BoundingConstraint2: public NonlinearInequalityConstraint {
|
||||
typedef VALUE1 X1;
|
||||
typedef VALUE2 X2;
|
||||
|
||||
typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
|
||||
typedef NonlinearInequalityConstraint Base;
|
||||
typedef std::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
|
||||
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
double threshold_;
|
||||
bool isGreaterThan_; /// flag for greater/less than
|
||||
|
||||
BoundingConstraint2(Key key1, Key key2, double threshold,
|
||||
bool isGreaterThan, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
|
||||
: Base(noiseModel::Constrained::All(1, mu), KeyVector{key1, key2}),
|
||||
threshold_(threshold), isGreaterThan_(isGreaterThan) {}
|
||||
|
||||
~BoundingConstraint2() override {}
|
||||
|
|
@ -133,15 +138,27 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
|
|||
OptionalMatrixType H1 = OptionalNone,
|
||||
OptionalMatrixType H2 = OptionalNone) const = 0;
|
||||
|
||||
/** active when constraint *NOT* met */
|
||||
bool active(const Values& c) const override {
|
||||
// note: still active at equality to avoid zigzagging
|
||||
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
|
||||
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
|
||||
Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override {
|
||||
X1 x1 = x.at<X1>(keys().front());
|
||||
X2 x2 = x.at<X2>(keys().back());
|
||||
if (H) {
|
||||
double d = value(x1, x2, &(H->front()), &(H->back()));
|
||||
if (isGreaterThan_) {
|
||||
H->front() *= -1;
|
||||
H->back() *= -1;
|
||||
return Vector1(threshold_ - d);
|
||||
} else {
|
||||
return Vector1(d - threshold_);
|
||||
}
|
||||
} else {
|
||||
double d = value(x1, x2);
|
||||
return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_);
|
||||
}
|
||||
}
|
||||
|
||||
/// TODO: This should be deprecated.
|
||||
Vector evaluateError(const X1& x1, const X2& x2,
|
||||
OptionalMatrixType H1, OptionalMatrixType H2) const override {
|
||||
OptionalMatrixType H1 = {}, OptionalMatrixType H2 = {}) const {
|
||||
Matrix D1, D2;
|
||||
double error = value(x1, x2, &D1, &D2) - threshold_;
|
||||
if (H1) {
|
||||
|
|
|
|||
|
|
@ -90,8 +90,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) {
|
|||
EXPECT(constraint2.active(config));
|
||||
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol));
|
||||
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol));
|
||||
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol));
|
||||
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol));
|
||||
EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint1.unwhitenedError(config), tol));
|
||||
EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint2.unwhitenedError(config), tol));
|
||||
EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
|
||||
EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
|
||||
}
|
||||
|
|
@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
|
|||
config.insert(key, pt1);
|
||||
EXPECT(constraint3.active(config));
|
||||
EXPECT(constraint4.active(config));
|
||||
EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
|
||||
EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
|
||||
EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
|
||||
EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
|
||||
// EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
|
||||
// EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
|
||||
EXPECT(assert_equal(1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
|
||||
EXPECT(assert_equal(1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
|
||||
EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
|
||||
EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
|
||||
}
|
||||
|
|
@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
|
|||
|
||||
config1.update(key2, pt4);
|
||||
EXPECT(rangeBound.active(config1));
|
||||
EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1)));
|
||||
EXPECT(assert_equal(1.0*I_1x1, rangeBound.unwhitenedError(config1)));
|
||||
EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue