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

Allow to pass in pre-computed generators. Should save some energy.

release/4.3a0
Frank Dellaert 2020-08-01 15:43:55 -04:00
parent e22c24eff5
commit a4590a2fe3
3 changed files with 107 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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();