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

Merge branch 'develop' of github.com:borglab/gtsam into feature/essential-mat-with-approx-k

release/4.3a0
Akshay Krishnan 2021-06-08 05:11:56 +00:00
parent 71b9004a2d 4270399083
commit bf93f1763c
30 changed files with 3468 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/geometry/Cal3Bundler.cpp
View File

@ -96,23 +96,27 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2
// but specialized with k1, k2 non-zero only and fx=fy and s=0
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for
// pixels around boundary
Point2 pn(x, y);
double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
const Point2 invKPi(px, py);
Point2 pn;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol_) break;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
int iteration = 0;
do {
// initialize pn with distortion included
const double rr = (px * px) + (py * py);
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
}
if (distance2(uncalibrate(pn), pi) <= tol_) break;
// Set px and py using intrinsic coordinates since that is where radial
// distortion correction is done.
px = pn.x(), py = pn.y();
iteration++;
} while (iteration < maxIterations);
if (iteration >= maxIterations)
throw std::runtime_error(
@ -148,4 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H;
}
} // \ namespace gtsam
} // namespace gtsam

98
gtsam/geometry/CameraSet.h
View File

@ -69,9 +69,12 @@ protected:
public:
/// Destructor
virtual ~CameraSet() = default;
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
using MatrixZD = Eigen::Matrix<double, ZDim, D>;
using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
/**
* print
@ -139,6 +142,57 @@ public:
return ErrorVector(project2(point, Fs, E), measured);
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* Fixed size version
*/
template<int N, int ND> // N = 2 or 3, ND is the camera dimension
static SymmetricBlockMatrix SchurComplement(
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
@ -148,45 +202,7 @@ public:
template<int N> // N = 2 or 3
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
return SchurComplement<N,D>(Fs, E, P, b);
}
/// Computes Point Covariance P, with lambda parameter

13
gtsam/geometry/Point2.h
View File

@ -82,5 +82,18 @@ GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, bo
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);
template <typename A1, typename A2>
struct Range;
template <>
struct Range<Point2, Point2> {
typedef double result_type;
double operator()(const Point2& p, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none) {
return distance2(p, q, H1, H2);
}
};
} // \ namespace gtsam

3
gtsam/geometry/SO3.cpp
View File

@ -278,7 +278,8 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
// see https://github.com/borglab/gtsam/issues/746 for details
magnitude = 0.5 - tr_3 / 12.0;
}
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}

54
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -62,6 +62,60 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
Point2 expected = p;
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = trueK.uncalibrate(pn);
Point2 actual = trueK.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
Cal3Bundler K(5, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(12, 17);
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibratePrincipalPoint) {
Cal3Bundler K(2, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 actual = K.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp;

28
gtsam/gtsam.i
View File

@ -2166,6 +2166,34 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor {
Vector whitenedError(const gtsam::Values& x) const;
};
#include <gtsam/nonlinear/CustomFactor.h>
virtual class CustomFactor: gtsam::NoiseModelFactor {
/*
* Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there.
* This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`.
*/
CustomFactor();
/*
* Example:
* ```
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* H[0] = J1 # 2-d numpy array for a Jacobian block
* H[1] = J2
* ...
* return error # 1-d numpy array
*
* cf = CustomFactor(noise_model, keys, error_func)
* ```
*/
CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys,
const gtsam::CustomErrorFunction& errorFunction);
void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
};
#include <gtsam/nonlinear/Values.h>
class Values {
Values();

76
gtsam/nonlinear/CustomFactor.cpp Normal file
View File

@ -0,0 +1,76 @@
/* ----------------------------------------------------------------------------
* 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 CustomFactor.cpp
* @brief Class to enable arbitrary factors with runtime swappable error function.
* @author Fan Jiang
*/
#include <gtsam/nonlinear/CustomFactor.h>
namespace gtsam {
/*
* Calculates the unwhitened error by invoking the callback functor (i.e. from Python).
*/
Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
if(this->active(x)) {
if(H) {
/*
* In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
* As the type `std::vector<Matrix>` has been marked as opaque in `preamble.h`, any changes in
* Python will be immediately reflected on the C++ side.
*
* Example:
* ```
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* H[0] = J1
* H[1] = J2
* ...
* return error
* ```
*/
return this->error_function_(*this, x, H.get_ptr());
} else {
/*
* In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.
* Users can check for `None` in their callback to determine if the Jacobian is requested.
*/
return this->error_function_(*this, x, nullptr);
}
} else {
return Vector::Zero(this->dim());
}
}
void CustomFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
std::cout << s << "CustomFactor on ";
auto keys_ = this->keys();
bool f = false;
for (const Key &k: keys_) {
if (f)
std::cout << ", ";
std::cout << keyFormatter(k);
f = true;
}
std::cout << "\n";
if (this->noiseModel_)
this->noiseModel_->print(" noise model: ");
else
std::cout << "no noise model" << std::endl;
}
}

104
gtsam/nonlinear/CustomFactor.h Normal file
View File

@ -0,0 +1,104 @@
/* ----------------------------------------------------------------------------
* 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 CustomFactor.h
* @brief Class to enable arbitrary factors with runtime swappable error function.
* @author Fan Jiang
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
using namespace gtsam;
namespace gtsam {
using JacobianVector = std::vector<Matrix>;
class CustomFactor;
/*
* NOTE
* ==========
* pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
*
* This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
* Thus the pointer will never be invalidated.
*/
using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
/**
* @brief Custom factor that takes a std::function as the error
* @addtogroup nonlinear
* \nosubgrouping
*
* This factor is mainly for creating a custom factor in Python.
*/
class CustomFactor: public NoiseModelFactor {
protected:
CustomErrorFunction error_function_;
protected:
using Base = NoiseModelFactor;
using This = CustomFactor;
public:
/**
* Default Constructor for I/O
*/
CustomFactor() = default;
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param keys keys of the variables
* @param errorFunction the error functional
*/
CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
Base(noiseModel, keys) {
this->error_function_ = errorFunction;
}
~CustomFactor() override = default;
/**
* Calls the errorFunction closure, which is a std::function object
* One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
*/
Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
/** print */
void print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/**
* Mark not sendable
*/
bool sendable() const override {
return false;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("CustomFactor",
boost::serialization::base_object<Base>(*this));
}
};
};

75
gtsam/nonlinear/GncOptimizer.h
View File

@ -74,6 +74,35 @@ class GncOptimizer {
}
}
// check that known inliers and outliers make sense:
std::vector<size_t> inconsistentlySpecifiedWeights; // measurements the user has incorrectly specified
// to be BOTH known inliers and known outliers
std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(),
params.knownOutliers.begin(),params.knownOutliers.end(),
std::inserter(inconsistentlySpecifiedWeights, inconsistentlySpecifiedWeights.begin()));
if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently specified weights, we throw an exception
params.print("params\n");
throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
" to be BOTH a known inlier and a known outlier.");
}
// check that known inliers are in the graph
for (size_t i = 0; i < params.knownInliers.size(); i++){
if( params.knownInliers[i] > nfg_.size()-1 ){ // outside graph
throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
"that are not in the factor graph to be known inliers.");
}
}
// check that known outliers are in the graph
for (size_t i = 0; i < params.knownOutliers.size(); i++){
if( params.knownOutliers[i] > nfg_.size()-1 ){ // outside graph
throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
"that are not in the factor graph to be known outliers.");
}
}
// initialize weights (if we don't have prior knowledge of inliers/outliers
// the weights are all initialized to 1.
weights_ = initializeWeightsFromKnownInliersAndOutliers();
// set default barcSq_ (inlier threshold)
double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
setInlierCostThresholdsAtProbability(alpha);
@ -109,6 +138,17 @@ class GncOptimizer {
}
}
/** Set weights for each factor. This is typically not needed, but
* provides an extra interface for the user to initialize the weightst
* */
void setWeights(const Vector w) {
if(w.size() != nfg_.size()){
throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights"
" does not match the size of the factor graph.");
}
weights_ = w;
}
/// Access a copy of the internal factor graph.
const NonlinearFactorGraph& getFactors() const { return nfg_; }
@ -132,26 +172,39 @@ class GncOptimizer {
&& equal(barcSq_, other.getInlierCostThresholds());
}
Vector initializeWeightsFromKnownInliersAndOutliers() const{
Vector weights = Vector::Ones(nfg_.size());
for (size_t i = 0; i < params_.knownOutliers.size(); i++){
weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers
}
return weights;
}
/// Compute optimal solution using graduated non-convexity.
Values optimize() {
// start by assuming all measurements are inliers
weights_ = Vector::Ones(nfg_.size());
BaseOptimizer baseOptimizer(nfg_, state_);
NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
BaseOptimizer baseOptimizer(graph_initial, state_);
Values result = baseOptimizer.optimize();
double mu = initializeMu();
double prev_cost = nfg_.error(result);
double prev_cost = graph_initial.error(result);
double cost = 0.0; // this will be updated in the main loop
// handle the degenerate case that corresponds to small
// maximum residual errors at initialization
// For GM: if residual error is small, mu -> 0
// For TLS: if residual error is small, mu -> -1
if (mu <= 0) {
if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() );
// ^^ number of measurements that are not known to be inliers or outliers (GNC will need to figure them out)
if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this case
if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
std::cout << "GNC Optimizer stopped because maximum residual at "
"initialization is small."
<< std::endl;
}
if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
<< std::endl;
}
if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
result.print("result\n");
std::cout << "mu: " << mu << std::endl;
@ -350,17 +403,21 @@ class GncOptimizer {
/// Calculate gnc weights.
Vector calculateWeights(const Values& currentEstimate, const double mu) {
Vector weights = Vector::Ones(nfg_.size());
Vector weights = initializeWeightsFromKnownInliersAndOutliers();
// do not update the weights that the user has decided are known inliers
std::vector<size_t> allWeights;
for (size_t k = 0; k < nfg_.size(); k++) {
allWeights.push_back(k);
}
std::vector<size_t> knownWeights;
std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(),
params_.knownOutliers.begin(), params_.knownOutliers.end(),
std::inserter(knownWeights, knownWeights.begin()));
std::vector<size_t> unknownWeights;
std::set_difference(allWeights.begin(), allWeights.end(),
params_.knownInliers.begin(),
params_.knownInliers.end(),
knownWeights.begin(), knownWeights.end(),
std::inserter(unknownWeights, unknownWeights.begin()));
// update weights of known inlier/outlier measurements

21
gtsam/nonlinear/GncParams.h
View File

@ -71,6 +71,7 @@ class GncParams {
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
Verbosity verbosity = SILENT; ///< Verbosity level
std::vector<size_t> knownInliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are inliers
std::vector<size_t> knownOutliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are outliers
/// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
void setLossType(const GncLossType type) {
@ -112,8 +113,21 @@ class GncParams {
* only apply GNC to prune outliers from the loop closures.
* */
void setKnownInliers(const std::vector<size_t>& knownIn) {
for (size_t i = 0; i < knownIn.size(); i++)
for (size_t i = 0; i < knownIn.size(); i++){
knownInliers.push_back(knownIn[i]);
}
std::sort(knownInliers.begin(), knownInliers.end());
}
/** (Optional) Provide a vector of measurements that must be considered outliers. The enties in the vector
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
* and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
* */
void setKnownOutliers(const std::vector<size_t>& knownOut) {
for (size_t i = 0; i < knownOut.size(); i++){
knownOutliers.push_back(knownOut[i]);
}
std::sort(knownOutliers.begin(), knownOutliers.end());
}
/// Equals.
@ -121,7 +135,8 @@ class GncParams {
return baseOptimizerParams.equals(other.baseOptimizerParams)
&& lossType == other.lossType && maxIterations == other.maxIterations
&& std::fabs(muStep - other.muStep) <= tol
&& verbosity == other.verbosity && knownInliers == other.knownInliers;
&& verbosity == other.verbosity && knownInliers == other.knownInliers
&& knownOutliers == other.knownOutliers;
}
/// Print.
@ -144,6 +159,8 @@ class GncParams {
std::cout << "verbosity: " << verbosity << "\n";
for (size_t i = 0; i < knownInliers.size(); i++)
std::cout << "knownInliers: " << knownInliers[i] << "\n";
for (size_t i = 0; i < knownOutliers.size(); i++)
std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
baseOptimizerParams.print(str);
}
};

42
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -164,6 +164,48 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const {
return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor, linearizationPoint_));
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
const std::map<Key, Key>& rekey_mapping) const {
auto rekeyed_base_factor = Base::rekey(rekey_mapping);
// Update the keys to the properties as well
// Downncast so we have access to members
auto new_factor = boost::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
// Create a new Values to assign later
Values newLinearizationPoint;
for (size_t i = 0; i < factor_->size(); ++i) {
auto mapping = rekey_mapping.find(factor_->keys()[i]);
if (mapping != rekey_mapping.end()) {
new_factor->factor_->keys()[i] = mapping->second;
newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first));
}
}
new_factor->linearizationPoint_ = newLinearizationPoint;
// upcast back and return
return boost::static_pointer_cast<NonlinearFactor>(new_factor);
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
const KeyVector& new_keys) const {
auto rekeyed_base_factor = Base::rekey(new_keys);
// Update the keys to the properties as well
// Downncast so we have access to members
auto new_factor = boost::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
new_factor->factor_->keys() = new_factor->keys();
// Create a new Values to assign later
Values newLinearizationPoint;
for(size_t i=0; i<new_keys.size(); ++i) {
Key cur_key = linearizationPoint_->keys()[i];
newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key));
}
new_factor->linearizationPoint_ = newLinearizationPoint;
// upcast back and return
return boost::static_pointer_cast<NonlinearFactor>(new_factor);
}
/* ************************************************************************* */
NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph(
const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) {

15
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -120,8 +120,21 @@ public:
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
}
// casting syntactic sugar
/**
* Creates a shared_ptr clone of the
* factor with different keys using
* a map from old->new keys
*/
NonlinearFactor::shared_ptr rekey(
const std::map<Key, Key>& rekey_mapping) const override;
/**
* Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys
*/
NonlinearFactor::shared_ptr rekey(const KeyVector& new_keys) const override;
/// Casting syntactic sugar
inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); }
/**

14
gtsam/nonlinear/NonlinearFactor.h
View File

@ -95,7 +95,7 @@ public:
/**
* Checks whether a factor should be used based on a set of values.
* This is primarily used to implment inequality constraints that
* This is primarily used to implement inequality constraints that
* require a variable active set. For all others, the default implementation
* returning true solves this problem.
*
@ -126,13 +126,21 @@ public:
* factor with different keys using
* a map from old->new keys
*/
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
/**
* Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys
*/
shared_ptr rekey(const KeyVector& new_keys) const;
virtual shared_ptr rekey(const KeyVector& new_keys) const;
/**
* Should the factor be evaluated in the same thread as the caller
* This is to enable factors that has shared states (like the Python GIL lock)
*/
virtual bool sendable() const {
return true;
}
}; // \class NonlinearFactor

12
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -325,7 +325,7 @@ public:
// Operator that linearizes a given range of the factors
void operator()(const tbb::blocked_range<size_t>& blocked_range) const {
for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) {
if (nonlinearGraph_[i])
if (nonlinearGraph_[i] && nonlinearGraph_[i]->sendable())
result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_);
else
result_[i] = GaussianFactor::shared_ptr();
@ -348,9 +348,19 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
linearFG->resize(size());
TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
// First linearize all sendable factors
tbb::parallel_for(tbb::blocked_range<size_t>(0, size()),
_LinearizeOneFactor(*this, linearizationPoint, *linearFG));
// Linearize all non-sendable factors
for(int i = 0; i < size(); i++) {
auto& factor = (*this)[i];
if(factor && !(factor->sendable())) {
(*linearFG)[i] = factor->linearize(linearizationPoint);
}
}
#else
linearFG->reserve(size());

98
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -6,13 +6,15 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <boost/assign/std/vector.hpp>
using namespace std;
@ -28,7 +30,7 @@ Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) {
TEST(TestLinearContainerFactor, generic_jacobian_factor) {
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
@ -61,7 +63,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
TEST(TestLinearContainerFactor, jacobian_factor_withlinpoints) {
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
@ -115,7 +117,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_hessian_factor ) {
TEST(TestLinearContainerFactor, generic_hessian_factor) {
Matrix G11 = (Matrix(1, 1) << 1.0).finished();
Matrix G12 = (Matrix(1, 2) << 2.0, 4.0).finished();
Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished();
@ -153,7 +155,7 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
TEST(TestLinearContainerFactor, hessian_factor_withlinpoints) {
// 2 variable example, one pose, one landmark (planar)
// Initial ordering: x1, l1
@ -226,7 +228,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, creation ) {
TEST(TestLinearContainerFactor, Creation) {
// Create a set of local keys (No robot label)
Key l1 = 11, l3 = 13, l5 = 15;
@ -252,7 +254,7 @@ TEST( testLinearContainerFactor, creation ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_relinearize )
TEST(TestLinearContainerFactor, jacobian_relinearize)
{
// Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1);
@ -286,7 +288,7 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, hessian_relinearize )
TEST(TestLinearContainerFactor, hessian_relinearize)
{
// Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1);
@ -319,6 +321,76 @@ TEST( testLinearContainerFactor, hessian_relinearize )
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
}
/* ************************************************************************* */
TEST(TestLinearContainerFactor, Rekey) {
// Make an example factor
auto nonlinear_factor =
boost::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0),
gtsam::noiseModel::Isotropic::Sigma(3, 1));
// Linearize and create an LCF
gtsam::Values linearization_pt;
linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3(0, 0, 0));
linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0));
LinearContainerFactor lcf_factor(
nonlinear_factor->linearize(linearization_pt), linearization_pt);
// Define a key mapping
std::map<gtsam::Key, gtsam::Key> key_map;
key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4);
key_map[gtsam::Symbol('l', 0)] = gtsam::Symbol('l', 4);
// Rekey (Calls NonlinearFactor::rekey() which should probably be overriden)
// This of type boost_ptr<NonlinearFactor>
auto lcf_factor_rekeyed = lcf_factor.rekey(key_map);
// Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
boost::static_pointer_cast<LinearContainerFactor>(lcf_factor_rekeyed);
CHECK(lcf_factor_rekey_ptr);
// For extra fun lets try linearizing this LCF
gtsam::Values linearization_pt_rekeyed;
for (auto key_val : linearization_pt) {
linearization_pt_rekeyed.insert(key_map.at(key_val.key), key_val.value);
}
// Check independent values since we don't want to unnecessarily sort
// The keys are just in the reverse order wrt the other container
CHECK(assert_equal(linearization_pt_rekeyed.keys()[1], lcf_factor_rekey_ptr->keys()[0]));
CHECK(assert_equal(linearization_pt_rekeyed.keys()[0], lcf_factor_rekey_ptr->keys()[1]));
}
/* ************************************************************************* */
TEST(TestLinearContainerFactor, Rekey2) {
// Make an example factor
auto nonlinear_factor =
boost::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0),
gtsam::noiseModel::Isotropic::Sigma(3, 1));
// Linearize and create an LCF
gtsam::Values linearization_pt;
linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3(0, 0, 0));
linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0));
LinearContainerFactor lcf_factor(
nonlinear_factor->linearize(linearization_pt), linearization_pt);
// Define a key mapping with only a single key remapped.
// This should throw an exception if there is a bug.
std::map<gtsam::Key, gtsam::Key> key_map;
key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4);
// Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
boost::static_pointer_cast<LinearContainerFactor>(
lcf_factor.rekey(key_map));
CHECK(lcf_factor_rekey_ptr);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

37
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -40,9 +40,9 @@ typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
// Keys are deliberately *not* in sorted order to test that case.
Key poseKey(2);
Key pointKey(1);
double measurement(10.0);
constexpr Key poseKey(2);
constexpr Key pointKey(1);
constexpr double measurement(10.0);
/* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point,
@ -364,20 +364,37 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
}
/* ************************************************************************* */
// Do a test with Point3
TEST(RangeFactor, Point3) {
// Do a test with Point2
TEST(RangeFactor, Point2) {
// Create a factor
RangeFactor<Point3> factor(poseKey, pointKey, measurement, model);
RangeFactor<Point2> factor(11, 22, measurement, model);
// Set the linearization point
Point3 pose(1.0, 2.0, 00);
Point3 point(-4.0, 11.0, 0);
Point2 p11(1.0, 2.0), p22(-4.0, 11.0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter
Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error
CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
Values values {{11, genericValue(p11)}, {22, genericValue(p22)}};
CHECK(assert_equal(expectedError, factor.unwhitenedError(values), 1e-9));
}
/* ************************************************************************* */
// Do a test with Point3
TEST(RangeFactor, Point3) {
// Create a factor
RangeFactor<Point3> factor(11, 22, measurement, model);
// Set the linearization point
Point3 p11(1.0, 2.0, 0.0), p22(-4.0, 11.0, 0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter
Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error
Values values {{11, genericValue(p11)}, {22, genericValue(p22)}};
CHECK(assert_equal(expectedError, factor.unwhitenedError(values), 1e-9));
}
/* ************************************************************************* */

98
gtsam_unstable/examples/GncPoseAveragingExample.cpp Normal file
View File

@ -0,0 +1,98 @@
/* ----------------------------------------------------------------------------
* 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 GncPoseAveragingExample.cpp
* @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers
* You can run this example using: ./GncPoseAveragingExample nrInliers nrOutliers
* e.g.,: ./GncPoseAveragingExample 10 5 (if the numbers are not specified, default
* values nrInliers = 10 and nrOutliers = 10 are used)
* @date May 8, 2021
* @author Luca Carlone
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/GncOptimizer.h>
#include <string>
#include <fstream>
#include <iostream>
#include <random>
#include <boost/lexical_cast.hpp>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
cout << "== Robust Pose Averaging Example === " << endl;
// default number of inliers and outliers
size_t nrInliers = 10;
size_t nrOutliers = 10;
// User can pass arbitrary number of inliers and outliers for testing
if (argc > 1)
nrInliers = atoi(argv[1]);
if (argc > 2)
nrOutliers = atoi(argv[2]);
cout << "nrInliers " << nrInliers << " nrOutliers "<< nrOutliers << endl;
// Seed random number generator
random_device rd;
mt19937 rng(rd());
uniform_real_distribution<double> uniform(-10, 10);
normal_distribution<double> normalInliers(0.0, 0.05);
Values initial;
initial.insert(0, Pose3::identity()); // identity pose as initialization
// create ground truth pose
Vector6 poseGtVector;
for(size_t i = 0; i < 6; ++i){
poseGtVector(i) = uniform(rng);
}
Pose3 gtPose = Pose3::Expmap(poseGtVector); // Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) );
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(6,0.05);
// create inliers
for(size_t i=0; i<nrInliers; i++){
Vector6 poseNoise;
for(size_t i = 0; i < 6; ++i){
poseNoise(i) = normalInliers(rng);
}
Pose3 poseMeasurement = gtPose.retract(poseNoise);
graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
}
// create outliers
for(size_t i=0; i<nrOutliers; i++){
Vector6 poseNoise;
for(size_t i = 0; i < 6; ++i){
poseNoise(i) = uniform(rng);
}
Pose3 poseMeasurement = gtPose.retract(poseNoise);
graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
}
GncParams<LevenbergMarquardtParams> gncParams;
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(graph,
initial,
gncParams);
Values estimate = gnc.optimize();
Pose3 poseError = gtPose.between( estimate.at<Pose3>(0) );
cout << "norm of translation error: " << poseError.translation().norm() <<
" norm of rotation error: " << poseError.rotation().rpy().norm() << endl;
// poseError.print("pose error: \n ");
return 0;
}

141
gtsam_unstable/slam/MagPoseFactor.h Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
* This version uses the measurement model bM = scale * bRn * direction + bias,
* where bRn is the rotation of the body in the nav frame, and scale, direction,
* and bias are assumed to be known. If the factor is constructed with a
* body_P_sensor, then the magnetometer measurements and bias should be
* expressed in the sensor frame.
*/
template <class POSE>
class MagPoseFactor: public NoiseModelFactor1<POSE> {
private:
using This = MagPoseFactor<POSE>;
using Base = NoiseModelFactor1<POSE>;
using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE.
using Rot = typename POSE::Rotation;
const Point measured_; ///< The measured magnetometer data in the body frame.
const Point nM_; ///< Local magnetic field (mag output units) in the nav frame.
const Point bias_; ///< The bias vector (mag output units) in the body frame.
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
static const int MeasDim = Point::RowsAtCompileTime;
static const int PoseDim = traits<POSE>::dimension;
static const int RotDim = traits<Rot>::dimension;
/// Shorthand for a smart pointer to a factor.
using shared_ptr = boost::shared_ptr<MagPoseFactor<POSE>>;
/// Concept check by type.
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE)
public:
~MagPoseFactor() override {}
/// Default constructor - only use for serialization.
MagPoseFactor() {}
/**
* Construct the factor.
* @param pose_key of the unknown pose nPb in the factor graph
* @param measured magnetometer reading, a Point2 or Point3
* @param scale by which a unit vector is scaled to yield a magnetometer reading
* @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
* @param bias of the magnetometer, modeled as purely additive (after scaling)
* @param model of the additive Gaussian noise that is assumed
* @param body_P_sensor an optional transform of the magnetometer in the body frame
*/
MagPoseFactor(Key pose_key,
const Point& measured,
double scale,
const Point& direction,
const Point& bias,
const SharedNoiseModel& model,
const boost::optional<POSE>& body_P_sensor)
: Base(model, pose_key),
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
nM_(scale * direction.normalized()),
bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias),
body_P_sensor_(body_P_sensor) {}
/// @return a deep copy of this factor.
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this)));
}
/// Implement functions needed for Testable.
// Print out the factor.
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
gtsam::print(Vector(nM_), "local field (nM): ");
gtsam::print(Vector(measured_), "measured field (bM): ");
gtsam::print(Vector(bias_), "magnetometer bias: ");
}
/// Equals function.
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) &&
gtsam::equal_with_abs_tol(this->measured_, e->measured_, tol) &&
gtsam::equal_with_abs_tol(this->nM_, e->nM_, tol) &&
gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol);
}
/// Implement functions needed to derive from Factor.
/**
* Return the factor's error h(x) - z, and the optional Jacobian. Note that
* the measurement error is expressed in the body frame.
*/
Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
// Predict the measured magnetic field h(x) in the *body* frame.
// If body_P_sensor was given, bias_ will have been rotated into the body frame.
Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
if (H) {
// Fill in the relevant part of the Jacobian (just rotation columns).
*H = Matrix::Zero(MeasDim, PoseDim);
const size_t rot_col0 = nPb.rotationInterval().first;
(*H).block(0, rot_col0, MeasDim, RotDim) = H_rot;
}
return (hx - measured_);
}
private:
/// Serialization function.
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(nM_);
ar & BOOST_SERIALIZATION_NVP(bias_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // \class MagPoseFactor
} /// namespace gtsam

4
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -450,8 +450,8 @@ public:
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
*/
void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const override
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const override
{
// when using stereo cameras, some of the measurements might be missing:
for(size_t i=0; i < cameras.size(); i++){

125
gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp Normal file
View File

@ -0,0 +1,125 @@
/* ----------------------------------------------------------------------------
* 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 SmartStereoProjectionFactorPP.h
* @brief Smart stereo factor on poses and extrinsic calibration
* @author Luca Carlone
* @author Frank Dellaert
*/
#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
namespace gtsam {
SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params)
: Base(sharedNoiseModel, params) {} // note: no extrinsic specified!
void SmartStereoProjectionFactorPP::add(
const StereoPoint2& measured,
const Key& w_P_body_key, const Key& body_P_cam_key,
const boost::shared_ptr<Cal3_S2Stereo>& K) {
// we index by cameras..
Base::add(measured, w_P_body_key);
// but we also store the extrinsic calibration keys in the same order
world_P_body_keys_.push_back(w_P_body_key);
body_P_cam_keys_.push_back(body_P_cam_key);
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end())
keys_.push_back(body_P_cam_key); // add only unique keys
K_all_.push_back(K);
}
void SmartStereoProjectionFactorPP::add(
const std::vector<StereoPoint2>& measurements,
const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) {
assert(world_P_body_keys.size() == measurements.size());
assert(world_P_body_keys.size() == body_P_cam_keys.size());
assert(world_P_body_keys.size() == Ks.size());
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements[i], world_P_body_keys[i]);
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
world_P_body_keys_.push_back(world_P_body_keys[i]);
body_P_cam_keys_.push_back(body_P_cam_keys[i]);
K_all_.push_back(Ks[i]);
}
}
void SmartStereoProjectionFactorPP::add(
const std::vector<StereoPoint2>& measurements,
const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
const boost::shared_ptr<Cal3_S2Stereo>& K) {
assert(world_P_body_keys.size() == measurements.size());
assert(world_P_body_keys.size() == body_P_cam_keys.size());
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements[i], world_P_body_keys[i]);
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
world_P_body_keys_.push_back(world_P_body_keys[i]);
body_P_cam_keys_.push_back(body_P_cam_keys[i]);
K_all_.push_back(K);
}
}
void SmartStereoProjectionFactorPP::print(
const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << "SmartStereoProjectionFactorPP: \n ";
for (size_t i = 0; i < K_all_.size(); i++) {
K_all_[i]->print("calibration = ");
std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl;
}
Base::print("", keyFormatter);
}
bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p,
double tol) const {
const SmartStereoProjectionFactorPP* e =
dynamic_cast<const SmartStereoProjectionFactorPP*>(&p);
return e && Base::equals(p, tol) &&
body_P_cam_keys_ == e->getExtrinsicPoseKeys();
}
double SmartStereoProjectionFactorPP::error(const Values& values) const {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
return 0.0;
}
}
SmartStereoProjectionFactorPP::Base::Cameras
SmartStereoProjectionFactorPP::cameras(const Values& values) const {
assert(world_P_body_keys_.size() == K_all_.size());
assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
Base::Cameras cameras;
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_[i]);
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_[i]);
Pose3 w_P_cam = w_P_body.compose(body_P_cam);
cameras.push_back(StereoCamera(w_P_cam, K_all_[i]));
}
return cameras;
}
} // \ namespace gtsam

369
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h Normal file
View File

@ -0,0 +1,369 @@
/* ----------------------------------------------------------------------------
* 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 SmartStereoProjectionFactorPP.h
* @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations
* @author Luca Carlone
* @author Frank Dellaert
*/
#pragma once
#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
* Eliminating conditionally independent sets in factor graphs:
* a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*/
/**
* This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body).
* Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras.
* This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables).
* @addtogroup SLAM
*/
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
protected:
/// shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
/// The keys corresponding to the pose of the body (with respect to an external world frame) for each view
KeyVector world_P_body_keys_;
/// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body)
KeyVector body_P_cam_keys_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartStereoProjectionFactor Base;
/// shorthand for this class
typedef SmartStereoProjectionFactorPP This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement)
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
/**
* Constructor
* @param Isotropic measurement noise
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams());
/** Virtual destructor */
~SmartStereoProjectionFactorPP() override = default;
/**
* add a new measurement, with a pose key, and an extrinsic pose key
* @param measured is the 3-dimensional location of the projection of a
* single landmark in the a single (stereo) view (the measurement)
* @param world_P_body_key is the key corresponding to the body poses observing the same landmark
* @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration
* @param K is the (fixed) camera intrinsic calibration
*/
void add(const StereoPoint2& measured, const Key& world_P_body_key,
const Key& body_P_cam_key,
const boost::shared_ptr<Cal3_S2Stereo>& K);
/**
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 3m dimensional location of the projection
* of a single landmark in the m (stereo) view (the measurements)
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
* @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
* (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
* @param Ks vector of intrinsic calibration objects
*/
void add(const std::vector<StereoPoint2>& measurements,
const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks);
/**
* Variant of the previous one in which we include a set of measurements with
* the same noise and calibration
* @param measurements vector of the 3m dimensional location of the projection
* of a single landmark in the m (stereo) view (the measurements)
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
* @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
* (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(const std::vector<StereoPoint2>& measurements,
const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
const boost::shared_ptr<Cal3_S2Stereo>& K);
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
/// equals
const KeyVector& getExtrinsicPoseKeys() const {
return body_P_cam_keys_;
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override;
/** return the calibration object */
inline std::vector<boost::shared_ptr<Cal3_S2Stereo>> calibration() const {
return K_all_;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding
* to keys involved in this factor
* @return vector of Values
*/
Base::Cameras cameras(const Values& values) const override;
/**
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the jacobian with
* respect to both the body pose and extrinsic pose), the point Jacobian E,
* and the error vector b. Note that the jacobians are computed for a given point.
*/
void computeJacobiansAndCorrectForMissingMeasurements(
FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const {
if (!result_) {
throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians
size_t numViews = measured_.size();
E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian)
b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view
Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei;
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_.at(i));
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i));
StereoCamera camera(
w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i),
K_all_[i]);
// get jacobians and error vector for current measurement
StereoPoint2 reprojectionError_i = StereoPoint2(
camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i));
Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
J.block<ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6)
J.block<ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6)
// if the right pixel is invalid, fix jacobians
if (std::isnan(measured_.at(i).uR()))
{
J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3);
reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0,
reprojectionError_i.v());
}
// fit into the output structures
Fs.push_back(J);
size_t row = 3 * i;
b.segment<ZDim>(row) = -reprojectionError_i.vector();
E.block<3, 3>(row, 0) = Ei;
}
}
}
/// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping =
false) const {
// we may have multiple cameras sharing the same extrinsic cals, hence the number
// of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
// have a body key and an extrinsic calibration key for each measurement)
size_t nrUniqueKeys = keys_.size();
size_t nrNonuniqueKeys = world_P_body_keys_.size()
+ body_P_cam_keys_.size();
// Create structures for Hessian Factors
KeyVector js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != cameras(values).size())
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point
triangulateSafe(cameras(values));
if (!result_) { // failed: return "empty/zero" Hessian
for (Matrix& m : Gs)
m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs)
v = Vector::Zero(DimPose);
return boost::make_shared < RegularHessianFactor<DimPose>
> (keys_, Gs, gs, 0.0);
}
// compute Jacobian given triangulated 3D Point
FBlocks Fs;
Matrix F, E;
Vector b;
computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
// Whiten using noise model
noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++)
Fs[i] = noiseModel_->Whiten(Fs[i]);
// build augmented Hessian (with last row/column being the information vector)
Matrix3 P;
Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
// marginalize point: note - we reuse the standard SchurComplement function
SymmetricBlockMatrix augmentedHessian =
Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
// now pack into an Hessian factor
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, 6);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessianUniqueKeys;
// here we have to deal with the fact that some cameras may share the same extrinsic key
if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix(augmentedHessian.selfadjointView()));
} else { // if multiple cameras share a calibration we have to rearrange
// the results of the Schur complement matrix
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix(
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys;
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
nonuniqueKeys.push_back(world_P_body_keys_.at(i));
nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
}
// get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
std::map<Key, size_t> keyToSlotMap;
for (size_t k = 0; k < nrUniqueKeys; k++) {
keyToSlotMap[keys_[k]] = k;
}
// initialize matrix to zero
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
// add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
// and populates an Hessian that only includes the unique keys (that is what we want to return)
for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
Key key_i = nonuniqueKeys.at(i);
// update information vector
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], nrUniqueKeys,
augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
// update blocks
for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
Key key_j = nonuniqueKeys.at(j);
if (i == j) {
augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
} else { // (i < j)
if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], keyToSlotMap[key_j],
augmentedHessian.aboveDiagonalBlock(i, j));
} else {
augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i],
augmentedHessian.aboveDiagonalBlock(i, j)
+ augmentedHessian.aboveDiagonalBlock(i, j).transpose());
}
}
}
}
// update bottom right element of the matrix
augmentedHessianUniqueKeys.updateDiagonalBlock(
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
}
return boost::make_shared < RegularHessianFactor<DimPose>
> (keys_, augmentedHessianUniqueKeys);
}
/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
* @param values Values structure which must contain camera poses and extrinsic pose for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(
const Values& values, const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) {
case HESSIAN:
return createHessianFactor(values, lambda);
default:
throw std::runtime_error(
"SmartStereoProjectionFactorPP: unknown linearization mode");
}
}
/// linearize
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
override {
return linearizeDamped(values);
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_all_);
}
};
// end of class declaration
/// traits
template<>
struct traits<SmartStereoProjectionFactorPP> : public Testable<
SmartStereoProjectionFactorPP> {
};
} // namespace gtsam

126
gtsam_unstable/slam/tests/testMagPoseFactor.cpp Normal file
View File

@ -0,0 +1,126 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam_unstable/slam/MagPoseFactor.h>
using namespace gtsam;
// *****************************************************************************
// Magnetic field in the nav frame (NED), with units of nT.
Point3 nM(22653.29982, -1956.83010, 44202.47862);
// Assumed scale factor (scales a unit vector to magnetometer units of nT).
double scale = 255.0 / 50000.0;
// Ground truth Pose2/Pose3 in the nav frame.
Pose3 n_P3_b = Pose3(Rot3::Yaw(-0.1), Point3(-3, 12, 5));
Pose2 n_P2_b = Pose2(Rot2(-0.1), Point2(-3, 12));
Rot3 n_R3_b = n_P3_b.rotation();
Rot2 n_R2_b = n_P2_b.rotation();
// Sensor bias (nT).
Point3 bias3(10, -10, 50);
Point2 bias2 = bias3.head(2);
Point3 dir3(nM.normalized());
Point2 dir2(nM.head(2).normalized());
// Compute the measured field in the sensor frame.
Point3 measured3 = n_R3_b.inverse() * (scale * dir3) + bias3;
// The 2D magnetometer will measure the "NE" field components.
Point2 measured2 = n_R2_b.inverse() * (scale * dir2) + bias2;
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.25);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25);
// Make up a rotation and offset of the sensor in the body frame.
Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0));
Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3));
// *****************************************************************************
// *****************************************************************************
TEST(MagPoseFactor, Constructors) {
MagPoseFactor<Pose2> f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
MagPoseFactor<Pose3> f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
// Try constructing with a body_P_sensor set.
MagPoseFactor<Pose2> f2b = MagPoseFactor<Pose2>(
Symbol('X', 0), measured2, scale, dir2, bias2, model2, body_P2_sensor);
MagPoseFactor<Pose3> f3b = MagPoseFactor<Pose3>(
Symbol('X', 0), measured3, scale, dir3, bias3, model3, body_P3_sensor);
f2b.print();
f3b.print();
}
// *****************************************************************************
TEST(MagPoseFactor, JacobianPose2) {
Matrix H2;
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
}
// *****************************************************************************
TEST(MagPoseFactor, JacobianPose3) {
Matrix H3;
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
}
// *****************************************************************************
TEST(MagPoseFactor, body_P_sensor2) {
Matrix H2;
// Because body_P_sensor is specified, we need to rotate the raw measurement
// from the body frame into the sensor frame "s".
const Rot2 nRs = n_R2_b * body_P2_sensor.rotation();
const Point2 sM = nRs.inverse() * (scale * dir2) + bias2;
MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
}
// *****************************************************************************
TEST(MagPoseFactor, body_P_sensor3) {
Matrix H3;
// Because body_P_sensor is specified, we need to rotate the raw measurement
// from the body frame into the sensor frame "s".
const Rot3 nRs = n_R3_b * body_P3_sensor.rotation();
const Point3 sM = nRs.inverse() * (scale * dir3) + bias3;
MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
}
// *****************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
// *****************************************************************************

1274
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

3
matlab/CMakeLists.txt
View File

@ -61,7 +61,8 @@ endif()
# ignoring the non-concrete types (type aliases)
set(ignore
gtsam::Point2
gtsam::Point3)
gtsam::Point3
gtsam::CustomFactor)
# Wrap
matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"

111
python/CustomFactors.md Normal file
View File

@ -0,0 +1,111 @@
# GTSAM Python-based factors
One now can build factors purely in Python using the `CustomFactor` factor.
## Usage
In order to use a Python-based factor, one needs to have a Python function with the following signature:
```python
import gtsam
import numpy as np
from typing import List
def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
...
```
`this` is a reference to the `CustomFactor` object. This is required because one can reuse the same
`error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of
**references** to the list of required Jacobians (see the corresponding C++ documentation).
If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error`
method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`,
each entry of `H` can be assigned a `numpy` array, as the Jacobian for the corresponding variable.
After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM:
```python
noise_model = gtsam.noiseModel.Unit.Create(3)
# constructor(<noise model>, <list of keys>, <error callback>)
cf = gtsam.CustomFactor(noise_model, [X(0), X(1)], error_func)
```
## Example
The following is a simple `BetweenFactor` implemented in Python.
```python
import gtsam
import numpy as np
from typing import List
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = gtsam.CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
```
In general, the Python-based factor works just like their C++ counterparts.
## Known Issues
Because of the `pybind11`-based translation, the performance of `CustomFactor` is not guaranteed.
Also, because `pybind11` needs to lock the Python GIL lock for evaluation of each factor, parallel
evaluation of `CustomFactor` is not possible.
## Implementation
`CustomFactor` is a `NonlinearFactor` that has a `std::function` as its callback.
This callback can be translated to a Python function call, thanks to `pybind11`'s functional support.
The constructor of `CustomFactor` is
```c++
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param keys keys of the variables
* @param errorFunction the error functional
*/
CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) :
Base(noiseModel, keys) {
this->error_function_ = errorFunction;
}
```
At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object.
Something worth special mention is this:
```c++
/*
* NOTE
* ==========
* pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
*
* This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
* Thus the pointer will never be invalidated.
*/
using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;
```
which is not documented in `pybind11` docs. One needs to be aware of this if they wanted to implement similar
"mutable" arguments going across the Python-C++ boundary.

179
python/gtsam/examples/CustomFactorExample.py Normal file
View File

@ -0,0 +1,179 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
CustomFactor demo that simulates a 1-D sensor fusion task.
Author: Fan Jiang, Frank Dellaert
"""
import gtsam
import numpy as np
from typing import List, Optional
from functools import partial
def simulate_car():
# Simulate a car for one second
x0 = 0
dt = 0.25 # 4 Hz, typical GPS
v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast
x = [x0 + v * dt * i for i in range(5)]
return x
x = simulate_car()
print(f"Simulated car trajectory: {x}")
# %%
add_noise = True # set this to False to run with "perfect" measurements
# GPS measurements
sigma_gps = 3.0 # assume GPS is +/- 3m
g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
for k in range(5)]
# Odometry measurements
sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz
o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0)
for k in range(4)]
# Landmark measurements:
sigma_lm = 1 # assume landmark measurement is accurate up to 1m
# Assume first landmark is at x=5, we measure it at time k=0
lm_0 = 5.0
z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
# Assume other landmark is at x=28, we measure it at time k=3
lm_3 = 28.0
z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
unknown = [gtsam.symbol('x', k) for k in range(5)]
print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
# We now can use nonlinear factor graphs
factor_graph = gtsam.NonlinearFactorGraph()
# Add factors for GPS measurements
I = np.eye(1)
gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""GPS Factor error function
:param measurement: GPS measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
estimate = values.atVector(key)
error = estimate - measurement
if jacobians is not None:
jacobians[0] = I
return error
# Add the GPS factors
for k in range(5):
gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]])))
factor_graph.add(gf)
# New Values container
v = gtsam.Values()
# Add initial estimates to the Values container
for i in range(5):
v.insert(unknown[i], np.array([0.0]))
# Initialize optimizer
params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
# Optimize the factor graph
result = optimizer.optimize()
# calculate the error from ground truth
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
print("Result with only GPS")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
# Adding odometry will improve things a lot
odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""Odometry Factor error function
:param measurement: Odometry measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key1 = this.keys()[0]
key2 = this.keys()[1]
pos1, pos2 = values.atVector(key1), values.atVector(key2)
error = measurement - (pos1 - pos2)
if jacobians is not None:
jacobians[0] = I
jacobians[1] = -I
return error
for k in range(4):
odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]])))
factor_graph.add(odof)
params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
result = optimizer.optimize()
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
print("Result with GPS+Odometry")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
# This is great, but GPS noise is still apparent, so now we add the two landmarks
lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""Landmark Factor error function
:param measurement: Landmark measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
pos = values.atVector(key)
error = pos - measurement
if jacobians is not None:
jacobians[0] = I
return error
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0]))))
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3]))))
params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
result = optimizer.optimize()
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
print("Result with GPS+Odometry+Landmark")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")

20
python/gtsam/preamble.h
View File

@ -1,14 +1,30 @@
// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
// These are required to save one copy operation on Python calls
/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
* These are required to save one copy operation on Python calls.
*
* NOTES
* =================
*
* `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
* such that the raw objects can be accessed in Python. Without this they will be automatically
* converted to a Python object, and all mutations on Python side will not be reflected on C++.
*/
#ifdef GTSAM_ALLOCATOR_TBB
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
#else
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
#endif
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>); // JacobianVector
// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this.
namespace std {
using gtsam::operator<<;
}

20
python/gtsam/specializations.h
View File

@ -1,10 +1,25 @@
// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
// These are required to save one copy operation on Python calls
/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
* These are required to save one copy operation on Python calls.
*
* NOTES
* =================
*
* `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
* such that the raw objects can be accessed in Python. Without this they will be automatically
* converted to a Python object, and all mutations on Python side will not be reflected on C++.
*
* `py::bind_vector` and similar machinery gives the std container a Python-like interface, but
* without the `<pybind11/stl.h>` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this
* allows the types to be modified with Python, and saves one copy operation.
*/
#ifdef GTSAM_ALLOCATOR_TBB
py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
py::implicitly_convertible<py::list, std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >();
#else
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
py::implicitly_convertible<py::list, std::vector<gtsam::Key> >();
#endif
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
@ -17,3 +32,4 @@ py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

207
python/gtsam/tests/test_custom_factor.py Normal file
View File

@ -0,0 +1,207 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
CustomFactor unit tests.
Author: Fan Jiang
"""
from typing import List
import unittest
from gtsam import Values, Pose2, CustomFactor
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestCustomFactor(GtsamTestCase):
def test_new(self):
"""Test the creation of a new CustomFactor"""
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func)
def test_new_keylist(self):
"""Test the creation of a new CustomFactor"""
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
def test_call(self):
"""Test if calling the factor works (only error)"""
expected_pose = Pose2(1, 1, 0)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray:
"""Minimal error function with no Jacobian"""
key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
v = Values()
v.insert(0, Pose2(1, 0, 0))
e = cf.error(v)
self.assertEqual(e, 0.5)
def test_jacobian(self):
"""Tests if the factor result matches the GTSAM Pose2 unit test"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""
the custom error function. One can freely use variables captured
from the outside scope. Or the variables can be acquired by indexing `v`.
Jacobian is passed by modifying the H array of numpy matrices.
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
gf = cf.linearize(v)
gf_b = bf.linearize(v)
J_cf, b_cf = gf.jacobian()
J_bf, b_bf = gf_b.jacobian()
np.testing.assert_allclose(J_cf, J_bf)
np.testing.assert_allclose(b_cf, b_bf)
def test_printing(self):
"""Tests if the factor result matches the GTSAM Pose2 unit test"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
from gtsam.symbol_shorthand import X
cf = CustomFactor(noise_model, [X(0), X(1)], error_func)
cf_string = """CustomFactor on x0, x1
noise model: unit (3)
"""
self.assertEqual(cf_string, repr(cf))
def test_no_jacobian(self):
"""Tests that we will not calculate the Jacobian if not requested"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
self.assertTrue(H is None) # Should be true if we only request the error
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
e_cf = cf.error(v)
e_bf = bf.error(v)
np.testing.assert_allclose(e_cf, e_bf)
def test_optimization(self):
"""Tests if a factor graph with a CustomFactor can be properly optimized"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)
fg = gtsam.NonlinearFactorGraph()
fg.add(cf)
fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model))
v = Values()
v.insert(0, Pose2(0, 0, 0))
v.insert(1, Pose2(0, 0, 0))
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params)
result = optimizer.optimize()
self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5)
self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
if __name__ == "__main__":
unittest.main()

173
tests/testGncOptimizer.cpp
View File

@ -660,7 +660,7 @@ TEST(GncOptimizer, barcsq_heterogeneousFactors) {
}
/* ************************************************************************* */
TEST(GncOptimizer, setWeights) {
TEST(GncOptimizer, setInlierCostThresholds) {
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
Point2 p0(1, 0);
@ -749,6 +749,177 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers!
}
/* ************************************************************************* */
TEST(GncOptimizer, knownInliersAndOutliers) {
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
Point2 p0(1, 0);
Values initial;
initial.insert(X(1), p0);
// nonconvexity with known inliers and known outliers (check early stopping
// when all measurements are known to be inliers or outliers)
{
std::vector<size_t> knownInliers;
knownInliers.push_back(0);
knownInliers.push_back(1);
knownInliers.push_back(2);
std::vector<size_t> knownOutliers;
knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams;
gncParams.setKnownInliers(knownInliers);
gncParams.setKnownOutliers(knownOutliers);
gncParams.setLossType(GncLossType::GM);
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams);
gnc.setInlierCostThresholds(1.0);
Values gnc_result = gnc.optimize();
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
// check weights were actually fixed:
Vector finalWeights = gnc.getWeights();
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
DOUBLES_EQUAL(1.0, finalWeights[1], tol);
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
DOUBLES_EQUAL(0.0, finalWeights[3], tol);
}
// nonconvexity with known inliers and known outliers
{
std::vector<size_t> knownInliers;
knownInliers.push_back(2);
knownInliers.push_back(0);
std::vector<size_t> knownOutliers;
knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams;
gncParams.setKnownInliers(knownInliers);
gncParams.setKnownOutliers(knownOutliers);
gncParams.setLossType(GncLossType::GM);
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams);
gnc.setInlierCostThresholds(1.0);
Values gnc_result = gnc.optimize();
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
// check weights were actually fixed:
Vector finalWeights = gnc.getWeights();
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
DOUBLES_EQUAL(1.0, finalWeights[1], 1e-5);
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
DOUBLES_EQUAL(0.0, finalWeights[3], tol);
}
// only known outliers
{
std::vector<size_t> knownOutliers;
knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams;
gncParams.setKnownOutliers(knownOutliers);
gncParams.setLossType(GncLossType::GM);
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams);
gnc.setInlierCostThresholds(1.0);
Values gnc_result = gnc.optimize();
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
// check weights were actually fixed:
Vector finalWeights = gnc.getWeights();
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
DOUBLES_EQUAL(1.0, finalWeights[1], 1e-5);
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
DOUBLES_EQUAL(0.0, finalWeights[3], tol);
}
}
/* ************************************************************************* */
TEST(GncOptimizer, setWeights) {
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
Point2 p0(1, 0);
Values initial;
initial.insert(X(1), p0);
// initialize weights to be the same
{
GncParams<GaussNewtonParams> gncParams;
gncParams.setLossType(GncLossType::TLS);
Vector weights = 0.5 * Vector::Ones(fg.size());
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams);
gnc.setWeights(weights);
gnc.setInlierCostThresholds(1.0);
Values gnc_result = gnc.optimize();
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
// check weights were actually fixed:
Vector finalWeights = gnc.getWeights();
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
DOUBLES_EQUAL(1.0, finalWeights[1], tol);
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
DOUBLES_EQUAL(0.0, finalWeights[3], tol);
}
// try a more challenging initialization
{
GncParams<GaussNewtonParams> gncParams;
gncParams.setLossType(GncLossType::TLS);
Vector weights = Vector::Zero(fg.size());
weights(2) = 1.0;
weights(3) = 1.0; // bad initialization: we say the outlier is inlier
// GNC can still recover (but if you omit weights(2) = 1.0, then it would fail)
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams);
gnc.setWeights(weights);
gnc.setInlierCostThresholds(1.0);
Values gnc_result = gnc.optimize();
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
// check weights were actually fixed:
Vector finalWeights = gnc.getWeights();
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
DOUBLES_EQUAL(1.0, finalWeights[1], tol);
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
DOUBLES_EQUAL(0.0, finalWeights[3], tol);
}
// initialize weights and also set known inliers/outliers
{
GncParams<GaussNewtonParams> gncParams;
std::vector<size_t> knownInliers;
knownInliers.push_back(2);
knownInliers.push_back(0);
std::vector<size_t> knownOutliers;
knownOutliers.push_back(3);
gncParams.setKnownInliers(knownInliers);
gncParams.setKnownOutliers(knownOutliers);
gncParams.setLossType(GncLossType::TLS);
Vector weights = 0.5 * Vector::Ones(fg.size());
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams);
gnc.setWeights(weights);
gnc.setInlierCostThresholds(1.0);
Values gnc_result = gnc.optimize();
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
// check weights were actually fixed:
Vector finalWeights = gnc.getWeights();
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
DOUBLES_EQUAL(1.0, finalWeights[1], tol);
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
DOUBLES_EQUAL(0.0, finalWeights[3], tol);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;