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

Merge pull request #453 from borglab/feature/better_frobenius_factors 

Better frobenius factors
release/4.3a0
Frank Dellaert 2020-08-02 16:13:18 -04:00 committed by GitHub
parent 8a3be052fe a4590a2fe3
commit 0e6b208276
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 189 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam.h
View File

@ -2369,6 +2369,7 @@ virtual class NonlinearOptimizer {
double error() const;
int iterations() const;
gtsam::Values values() const;
gtsam::NonlinearFactorGraph graph() const;
gtsam::GaussianFactorGraph* iterate() const;
};

10
gtsam/geometry/SOn-inl.h
View File

@ -26,13 +26,13 @@ using namespace std;
namespace gtsam {
// Implementation for N>5 just uses dynamic version
// Implementation for N>=5 just uses dynamic version
template <int N>
typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
return SOn::Hat(xi);
}
// Implementation for N>5 just uses dynamic version
// Implementation for N>=5 just uses dynamic version
template <int N>
typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
return SOn::Vee(X);
@ -99,12 +99,8 @@ typename SO<N>::VectorN2 SO<N>::vec(
if (H) {
// Calculate P matrix of vectorized generators
// TODO(duy): Should we refactor this as the jacobian of Hat?
Matrix P = VectorizedGenerators(n);
const size_t d = dim();
Matrix P(n2, d);
for (size_t j = 0; j < d; j++) {
const auto X = Hat(Eigen::VectorXd::Unit(d, j));
P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
H->resize(n2, d);
for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);

31
gtsam/geometry/SOn.h
View File

@ -290,7 +290,34 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* */
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
boost::none) const;
/// @}
/// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
template <int N_ = N, typename = IsFixed<N_>>
static Matrix VectorizedGenerators() {
constexpr size_t N2 = static_cast<size_t>(N * N);
Matrix G(N2, dimension);
for (size_t j = 0; j < dimension; j++) {
const auto X = Hat(Vector::Unit(dimension, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1);
}
return G;
}
/// Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
template <int N_ = N, typename = IsDynamic<N_>>
static Matrix VectorizedGenerators(size_t n = 0) {
const size_t n2 = n * n, dim = Dimension(n);
Matrix G(n2, dim);
for (size_t j = 0; j < dim; j++) {
const auto X = Hat(Vector::Unit(dim, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
return G;
}
/// @{
/// @name Serialization
/// @{
template <class Archive>
friend void save(Archive&, SO&, const unsigned int);
@ -300,6 +327,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access;
friend class Rot3; // for serialize
/// @}
};
using SOn = SO<Eigen::Dynamic>;

37
gtsam/nonlinear/NonlinearFactor.h
View File

@ -296,6 +296,8 @@ protected:
typedef NoiseModelFactor1<VALUE> This;
public:
/// @name Constructors
/// @{
/** Default constructor for I/O only */
NoiseModelFactor1() {}
@ -309,16 +311,23 @@ public:
* @param noiseModel shared pointer to noise model
* @param key1 by which to look up X value in Values
*/
NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
Base(noiseModel, cref_list_of<1>(key1)) {}
NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1)
: Base(noiseModel, cref_list_of<1>(key1)) {}
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class.
/// @}
/// @name NoiseModelFactor methods
/// @{
/**
* Calls the 1-key specific version of evaluateError below, which is pure
* virtual so must be implemented in the derived class.
*/
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) {
const X& x1 = x.at<X>(keys_[0]);
if(H) {
Vector unwhitenedError(
const Values &x,
boost::optional<std::vector<Matrix> &> H = boost::none) const override {
if (this->active(x)) {
const X &x1 = x.at<X>(keys_[0]);
if (H) {
return evaluateError(x1, (*H)[0]);
} else {
return evaluateError(x1);
@ -328,16 +337,22 @@ public:
}
}
/// @}
/// @name Virtual methods
/// @{
/**
* Override this method to finish implementing a unary factor.
* If the optional Matrix reference argument is specified, it should compute
* both the function evaluation and its derivative in X.
*/
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
virtual Vector
evaluateError(const X &x,
boost::optional<Matrix &> H = boost::none) const = 0;
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

11
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -105,14 +105,17 @@ public:
*/
const Values& optimizeSafely();
/// return error
/// return error in current optimizer state
double error() const;
/// return number of iterations
/// return number of iterations in current optimizer state
size_t iterations() const;
/// return values
const Values& values() const;
/// return values in current optimizer state
const Values &values() const;
/// return the graph with nonlinear factors
const NonlinearFactorGraph &graph() const { return graph_; }
/// @}

26
gtsam/slam/BetweenFactor.h
View File

@ -67,9 +67,11 @@ namespace gtsam {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/// @}
/// @name Testable
/// @{
/** print */
/// print with optional string
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << ","
@ -78,15 +80,17 @@ namespace gtsam {
this->noiseModel_->print(" noise model: ");
}
/** equals */
/// assert equality up to a tolerance
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */
/// @}
/// @name NoiseModelFactor2 methods
/// @{
/** vector of errors */
/// evaluate error, returns vector of errors size of tangent space
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
@ -102,15 +106,15 @@ namespace gtsam {
#endif
}
/** return the measured */
/// @}
/// @name Standard interface
/// @{
/// return the measurement
const VALUE& measured() const {
return measured_;
}
/** number of variables attached to this factor */
std::size_t size() const {
return 2;
}
/// @}
private:

5
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -87,11 +87,6 @@ public:
return measuredE_;
}
/** number of variables attached to this factor */
std::size_t size() const {
return 2;
}
private:
/** Serialization function */

51
gtsam/slam/FrobeniusFactor.cpp
View File

@ -52,23 +52,40 @@ boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
}
//******************************************************************************
FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12,
size_t p,
const SharedNoiseModel& model)
FrobeniusWormholeFactor::FrobeniusWormholeFactor(
Key j1, Key j2, const Rot3 &R12, size_t p, const SharedNoiseModel &model,
const boost::shared_ptr<Matrix> &G)
: NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
M_(R12.matrix()), // 3*3 in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
dimension_(SOn::Dimension(p)), // 6 for SO(4)
G_(pp_, dimension_) // 16*6 for SO(4)
{
// Calculate G matrix of vectorized generators
Matrix Z = Matrix::Zero(p, p);
for (size_t j = 0; j < dimension_; j++) {
const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j));
G_.col(j) = Eigen::Map<const Matrix>(X.data(), pp_, 1);
M_(R12.matrix()), // 3*3 in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
G_(G) {
if (noiseModel()->dim() != 3 * p_)
throw std::invalid_argument(
"FrobeniusWormholeFactor: model with incorrect dimension.");
if (!G) {
G_ = boost::make_shared<Matrix>();
*G_ = SOn::VectorizedGenerators(p); // expensive!
}
assert(noiseModel()->dim() == 3 * p_);
if (G_->rows() != pp_ || G_->cols() != SOn::Dimension(p))
throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators "
"of incorrect dimension.");
}
//******************************************************************************
void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
<< keyFormatter(key2()) << ")\n";
traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: ");
}
//******************************************************************************
bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected,
double tol) const {
auto e = dynamic_cast<const FrobeniusWormholeFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
p_ == e->p_ && M_ == e->M_;
}
//******************************************************************************
@ -98,7 +115,7 @@ Vector FrobeniusWormholeFactor::evaluateError(
RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0);
RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1);
RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2);
*H1 = -RPxQ * G_;
*H1 = -RPxQ * (*G_);
}
if (H2) {
const size_t p2 = 2 * p_;
@ -106,7 +123,7 @@ Vector FrobeniusWormholeFactor::evaluateError(
PxQ.block(0, 0, p_, p_) = M2;
PxQ.block(p_, p_, p_, p_) = M2;
PxQ.block(p2, p2, p_, p_) = M2;
*H2 = PxQ * G_;
*H2 = PxQ * (*G_);
}
return fQ2 - hQ1;

74
gtsam/slam/FrobeniusFactor.h
View File

@ -92,14 +92,17 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
* and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
*/
template <class Rot>
class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
GTSAM_EXPORT class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
Rot R12_; ///< measured rotation between R1 and R2
Eigen::Matrix<double, Rot::dimension, Rot::dimension>
R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
/// @name Constructor
/// @{
/// Construct from two keys and measured rotation
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(
@ -107,6 +110,33 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
/// @}
/// @name Testable
/// @{
/// print with optional string
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
<< ">(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
traits<Rot>::Print(R12_, " R12: ");
this->noiseModel_->print(" noise model: ");
}
/// assert equality up to a tolerance
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override {
auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) &&
traits<Rot>::Equals(this->R12_, e->R12_, tol);
}
/// @}
/// @name NoiseModelFactor2 methods
/// @{
/// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none,
@ -117,6 +147,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
return error;
}
/// @}
};
/**
@ -125,21 +156,46 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
* TODO(frank): template on D=2 or 3
*/
class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_, dimension_; ///< dimensionality constants
Matrix G_; ///< matrix of vectorized generators
class GTSAM_EXPORT FrobeniusWormholeFactor
: public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_; ///< dimensionality constants
boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
public:
/// @name Constructor
/// @{
public:
/// Constructor. Note we convert to 3*p-dimensional noise model.
FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4,
const SharedNoiseModel& model = nullptr);
/// To save memory and mallocs, pass in the vectorized Lie algebra generators:
/// G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
FrobeniusWormholeFactor(Key j1, Key j2, const Rot3 &R12, size_t p = 4,
const SharedNoiseModel &model = nullptr,
const boost::shared_ptr<Matrix> &G = nullptr);
/// @}
/// @name Testable
/// @{
/// print with optional string
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/// assert equality up to a tolerance
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override;
/// @}
/// @name NoiseModelFactor2 methods
/// @{
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector evaluateError(const SOn& Q1, const SOn& Q2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
/// @}
};
} // namespace gtsam

5
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -403,11 +403,6 @@ public:
return measured_;
}
/** number of variables attached to this factor */
std::size_t size() const {
return 2;
}
size_t dim() const override {
return model_inlier_->R().rows() + model_inlier_->R().cols();
}

5
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
View File

@ -203,11 +203,6 @@ namespace gtsam {
/* ************************************************************************* */
/** number of variables attached to this factor */
std::size_t size() const {
return 1;
}
size_t dim() const override {
return model_->R().rows() + model_->R().cols();
}

5
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -401,11 +401,6 @@ namespace gtsam {
/* ************************************************************************* */
/** number of variables attached to this factor */
std::size_t size() const {
return 1;
}
size_t dim() const override {
return model_inlier_->R().rows() + model_inlier_->R().cols();
}

19
timing/timeFrobeniusFactor.cpp
View File

@ -13,12 +13,11 @@
* @file timeFrobeniusFactor.cpp
* @brief time FrobeniusFactor with BAL file
* @author Frank Dellaert
* @date June 6, 2015
* @date 2019
*/
#include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
@ -51,10 +50,7 @@ int main(int argc, char* argv[]) {
if (argc > 1)
g2oFile = argv[argc - 1];
else
g2oFile =
"/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/"
"datasets/randomTorus3D.g2o";
// g2oFile = findExampleDataFile("sphere_smallnoise.graph");
g2oFile = findExampleDataFile("sphere_smallnoise.graph");
} catch (const exception& e) {
cerr << e.what() << '\n';
exit(1);
@ -66,15 +62,16 @@ int main(int argc, char* argv[]) {
// Build graph
NonlinearFactorGraph graph;
// graph.add(NonlinearEquality<SO4>(0, SO4()));
// graph.add(NonlinearEquality<SOn>(0, SOn::identity(4)));
auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
graph.add(PriorFactor<SO4>(0, SO4(), priorModel));
graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel));
auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4));
for (const auto& factor : factors) {
const auto& keys = factor->keys();
const auto& Tij = factor->measured();
const auto& model = factor->noiseModel();
graph.emplace_shared<FrobeniusWormholeFactor>(
keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model);
keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model, G);
}
std::mt19937 rng(42);
@ -95,9 +92,9 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < 100; i++) {
gttic_(optimize);
Values initial;
initial.insert(0, SO4());
initial.insert(0, SOn::identity(4));
for (size_t j = 1; j < poses.size(); j++) {
initial.insert(j, SO4::Random(rng));
initial.insert(j, SOn::Random(rng, 4));
}
LevenbergMarquardtOptimizer lm(graph, initial, params);
Values result = lm.optimize();