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

make check for entire library now works

release/4.3a0
Alex Cunningham 2011-11-04 21:44:34 +00:00
parent 07aaf38245
commit 97a82add25
32 changed files with 101 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/CalibratedCamera.h
View File

@ -69,12 +69,6 @@ namespace gtsam {
/// Return canonical coordinate /// Return canonical coordinate
static Vector Unretract(const CalibratedCamera& p); static Vector Unretract(const CalibratedCamera& p);
// // Manifold requirements - use existing expmap/logmap
// inline CalibratedCamera retract(const Vector& v) const { return expmap(v); }
// inline static CalibratedCamera Retract(const Vector& v) { return Expmap(v); }
// inline Vector unretract(const CalibratedCamera& t2) const { return logmap(t2); }
// inline static Vector Unretract(const CalibratedCamera& t) { return Logmap(t); }
/// Lie group dimensionality /// Lie group dimensionality
inline size_t dim() const { return 6 ; } inline size_t dim() const { return 6 ; }

8
gtsam/geometry/CalibratedCameraT.h
View File

@ -50,11 +50,11 @@ namespace gtsam {
return CalibratedCameraT( pose_.inverse(), k_ ) ; return CalibratedCameraT( pose_.inverse(), k_ ) ;
} }
CalibratedCameraT expmap(const Vector& d) const { CalibratedCameraT retract(const Vector& d) const {
return CalibratedCameraT(pose().expmap(d), k_) ; return CalibratedCameraT(pose().retract(d), k_) ;
} }
Vector logmap(const CalibratedCameraT& T2) const { Vector unretract(const CalibratedCameraT& T2) const {
return pose().logmap(T2.pose()) ; return pose().unretract(T2.pose()) ;
} }
void print(const std::string& s = "") const { void print(const std::string& s = "") const {

25
gtsam/geometry/GeneralCameraT.h
View File

@ -116,31 +116,28 @@ class GeneralCameraT {
calibration_.print(s + ".calibration.") ; calibration_.print(s + ".calibration.") ;
} }
GeneralCameraT expmap(const Vector &v) const { GeneralCameraT retract(const Vector &v) const {
Vector v1 = sub(v,0,Camera::Dim()); Vector v1 = sub(v,0,Camera::Dim());
Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
return GeneralCameraT(calibrated_.expmap(v1), calibration_.expmap(v2)); return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2));
} }
Vector logmap(const GeneralCameraT &C) const { Vector unretract(const GeneralCameraT &C) const {
//std::cout << "logmap" << std::endl; const Vector v1(calibrated().unretract(C.calibrated())),
const Vector v1(calibrated().logmap(C.calibrated())), v2(calibration().unretract(C.calibration()));
v2(calibration().logmap(C.calibration()));
return concatVectors(2,&v1,&v2) ; return concatVectors(2,&v1,&v2) ;
} }
static GeneralCameraT Expmap(const Vector& v) { static GeneralCameraT Retract(const Vector& v) {
//std::cout << "Expmap" << std::endl;
return GeneralCameraT( return GeneralCameraT(
Camera::Expmap(sub(v,0,Camera::Dim())), Camera::Retract(sub(v,0,Camera::Dim())),
Calibration::Expmap(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim())) Calibration::Retract(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim()))
); );
} }
static Vector Logmap(const GeneralCameraT& p) { static Vector Unretract(const GeneralCameraT& p) {
//std::cout << "Logmap" << std::endl; const Vector v1(Camera::Unretract(p.calibrated())),
const Vector v1(Camera::Logmap(p.calibrated())), v2(Calibration::Unretract(p.calibration()));
v2(Calibration::Logmap(p.calibration()));
return concatVectors(2,&v1,&v2); return concatVectors(2,&v1,&v2);
} }

2
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
// Extract the current estimate of x1,P1 from the Bayes Network // Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = optimize(*linearBayesNet); VectorValues result = optimize(*linearBayesNet);
T x = linearizationPoints[lastKey].expmap(result[lastIndex]); T x = linearizationPoints[lastKey].retract(result[lastIndex]);
// Create a Jacobian Factor from the root node of the produced Bayes Net. // Create a Jacobian Factor from the root node of the produced Bayes Net.
// This will act as a prior for the next iteration. // This will act as a prior for the next iteration.

2
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -237,7 +237,7 @@ struct _SelectiveExpmapAndClear {
assert(delta[var].size() == (int)it_x->second.dim()); assert(delta[var].size() == (int)it_x->second.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all()); assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) { if(mask[var]) {
it_x->second = it_x->second.expmap(delta[var]); it_x->second = it_x->second.retract(delta[var]);
if(invalidate) if(invalidate)
(*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?) (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
} }

2
gtsam/nonlinear/ISAM2-inl.h
View File

@ -548,7 +548,7 @@ template<class Conditional, class Values>
Values ISAM2<Conditional, Values>::calculateBestEstimate() const { Values ISAM2<Conditional, Values>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_)); VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta); optimize2(this->root(), delta);
return theta_.expmap(delta, ordering_); return theta_.retract(delta, ordering_);
} }
} }

16
gtsam/nonlinear/LieValues-inl.h
View File

@ -136,7 +136,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
template<class J> template<class J>
LieValues<J> LieValues<J>::expmap(const VectorValues& delta, const Ordering& ordering) const { LieValues<J> LieValues<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
LieValues<J> newValues; LieValues<J> newValues;
typedef pair<J,typename J::Value> KeyValue; typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) { BOOST_FOREACH(const KeyValue& value, this->values_) {
@ -144,7 +144,7 @@ namespace gtsam {
const typename J::Value& pj = value.second; const typename J::Value& pj = value.second;
Index index; Index index;
if(ordering.tryAt(j,index)) { if(ordering.tryAt(j,index)) {
newValues.insert(j, pj.expmap(delta[index])); newValues.insert(j, pj.retract(delta[index]));
} else } else
newValues.insert(j, pj); newValues.insert(j, pj);
} }
@ -171,7 +171,7 @@ namespace gtsam {
// /* ************************************************************************* */ // /* ************************************************************************* */
// // todo: insert for every element is inefficient // // todo: insert for every element is inefficient
// template<class J> // template<class J>
// LieValues<J> LieValues<J>::expmap(const Vector& delta) const { // LieValues<J> LieValues<J>::retract(const Vector& delta) const {
// if(delta.size() != dim()) { // if(delta.size() != dim()) {
// cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
// throw invalid_argument("Delta vector length does not match config dimensionality."); // throw invalid_argument("Delta vector length does not match config dimensionality.");
@ -183,7 +183,7 @@ namespace gtsam {
// const J& j = value.first; // const J& j = value.first;
// const typename J::Value& pj = value.second; // const typename J::Value& pj = value.second;
// int cur_dim = pj.dim(); // int cur_dim = pj.dim();
// newValues.insert(j,pj.expmap(sub(delta, delta_offset, delta_offset+cur_dim))); // newValues.insert(j,pj.retract(sub(delta, delta_offset, delta_offset+cur_dim)));
// delta_offset += cur_dim; // delta_offset += cur_dim;
// } // }
// return newValues; // return newValues;
@ -193,19 +193,19 @@ namespace gtsam {
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs // todo: currently only logmaps elements in both configs
template<class J> template<class J>
inline VectorValues LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering) const { inline VectorValues LieValues<J>::unretract(const LieValues<J>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta); unretract(cp, ordering, delta);
return delta; return delta;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const { void LieValues<J>::unretract(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const {
typedef pair<J,typename J::Value> KeyValue; typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first)); assert(this->exists(value.first));
delta[ordering[value.first]] = this->at(value.first).logmap(value.second); delta[ordering[value.first]] = this->at(value.first).unretract(value.second);
} }
} }

6
gtsam/nonlinear/LieValues.h
View File

@ -119,13 +119,13 @@ namespace gtsam {
// Lie operations // Lie operations
/** Add a delta config to current config and returns a new config */ /** Add a delta config to current config and returns a new config */
LieValues expmap(const VectorValues& delta, const Ordering& ordering) const; LieValues retract(const VectorValues& delta, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
VectorValues logmap(const LieValues& cp, const Ordering& ordering) const; VectorValues unretract(const LieValues& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
void logmap(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; void unretract(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const;
// imperative methods: // imperative methods:

4
gtsam/nonlinear/NonlinearConstraint.h
View File

@ -573,7 +573,7 @@ public:
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const { Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
const size_t p = X::Dim(); const size_t p = X::Dim();
if (H1) *H1 = eye(p); if (H1) *H1 = eye(p);
return value_.logmap(x1); return value_.unretract(x1);
} }
/** Print */ /** Print */
@ -628,7 +628,7 @@ public:
const size_t p = X::Dim(); const size_t p = X::Dim();
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
return x1.logmap(x2); return x1.unretract(x2);
} }
private: private:

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -120,7 +120,7 @@ namespace gtsam {
size_t nj = feasible_.dim(); size_t nj = feasible_.dim();
if (allow_error_) { if (allow_error_) {
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
return xj.logmap(feasible_); return xj.unretract(feasible_);
} else if (compare_(feasible_,xj)) { } else if (compare_(feasible_,xj)) {
if (H) *H = eye(nj); if (H) *H = eye(nj);
return zero(nj); // set error to zero if equal return zero(nj); // set error to zero if equal

2
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -93,7 +93,7 @@ void NonlinearISAM<Values>::reorder_relinearize() {
template<class Values> template<class Values>
Values NonlinearISAM<Values>::estimate() const { Values NonlinearISAM<Values>::estimate() const {
if(isam_.size() > 0) if(isam_.size() > 0)
return linPoint_.expmap(optimize(isam_), ordering_); return linPoint_.retract(optimize(isam_), ordering_);
else else
return linPoint_; return linPoint_;
} }

4
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -89,7 +89,7 @@ namespace gtsam {
if (verbosity >= Parameters::DELTA) delta.print("delta"); if (verbosity >= Parameters::DELTA) delta.print("delta");
// take old values and update it // take old values and update it
shared_values newValues(new C(values_->expmap(delta, *ordering_))); shared_values newValues(new C(values_->retract(delta, *ordering_)));
// maybe show output // maybe show output
if (verbosity >= Parameters::VALUES) newValues->print("newValues"); if (verbosity >= Parameters::VALUES) newValues->print("newValues");
@ -179,7 +179,7 @@ namespace gtsam {
if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
// update values // update values
shared_values newValues(new T(values_->expmap(delta, *ordering_))); shared_values newValues(new T(values_->retract(delta, *ordering_)));
// create new optimization state with more adventurous lambda // create new optimization state with more adventurous lambda
double error = graph_->error(*newValues); double error = graph_->error(*newValues);

26
gtsam/nonlinear/TupleValues.h
View File

@ -205,21 +205,21 @@ namespace gtsam {
} }
/** Expmap */ /** Expmap */
TupleValues<VALUES1, VALUES2> expmap(const VectorValues& delta, const Ordering& ordering) const { TupleValues<VALUES1, VALUES2> retract(const VectorValues& delta, const Ordering& ordering) const {
return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering)); return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering));
} }
/** logmap each element */ /** logmap each element */
VectorValues logmap(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering) const { VectorValues unretract(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta); unretract(cp, ordering, delta);
return delta; return delta;
} }
/** logmap each element */ /** logmap each element */
void logmap(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const { void unretract(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.logmap(cp.first_, ordering, delta); first_.unretract(cp.first_, ordering, delta);
second_.logmap(cp.second_, ordering, delta); second_.unretract(cp.second_, ordering, delta);
} }
/** /**
@ -318,18 +318,18 @@ namespace gtsam {
size_t dim() const { return first_.dim(); } size_t dim() const { return first_.dim(); }
TupleValuesEnd<VALUES> expmap(const VectorValues& delta, const Ordering& ordering) const { TupleValuesEnd<VALUES> retract(const VectorValues& delta, const Ordering& ordering) const {
return TupleValuesEnd(first_.expmap(delta, ordering)); return TupleValuesEnd(first_.retract(delta, ordering));
} }
VectorValues logmap(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const { VectorValues unretract(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta); unretract(cp, ordering, delta);
return delta; return delta;
} }
void logmap(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const { void unretract(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.logmap(cp.first_, ordering, delta); first_.unretract(cp.first_, ordering, delta);
} }
/** /**

6
gtsam/nonlinear/tests/testLieValues.cpp
View File

@ -137,7 +137,7 @@ TEST(LieValues, expmap_a)
expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.expmap(increment, ordering))); CHECK(assert_equal(expected, config0.retract(increment, ordering)));
} }
///* ************************************************************************* */ ///* ************************************************************************* */
@ -155,7 +155,7 @@ TEST(LieValues, expmap_a)
// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// //
// CHECK(assert_equal(expected, config0.expmap(increment, ordering))); // CHECK(assert_equal(expected, config0.retract(increment, ordering)));
//} //}
///* ************************************************************************* */ ///* ************************************************************************* */
@ -173,7 +173,7 @@ TEST(LieValues, expmap_a)
// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); // expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// //
// CHECK(assert_equal(expected, config0.expmap(increment))); // CHECK(assert_equal(expected, config0.retract(increment)));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/slam/BetweenConstraint.h
View File

@ -51,7 +51,7 @@ namespace gtsam {
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
X hx = x1.between(x2, H1, H2); X hx = x1.between(x2, H1, H2);
return measured_.logmap(hx); return measured_.unretract(hx);
} }
inline const X measured() const { inline const X measured() const {

2
gtsam/slam/BetweenFactor.h
View File

@ -83,7 +83,7 @@ namespace gtsam {
boost::none) const { boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x) T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return measured_.logmap(hx); return measured_.unretract(hx);
} }
/** return the measured */ /** return the measured */

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -82,7 +82,7 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> H2=boost::none) const {
Vector error = z_.logmap(camera.project(point,H1,H2)); Vector error = z_.unretract(camera.project(point,H1,H2));
// gtsam::print(error, "error"); // gtsam::print(error, "error");
return error; return error;
} }

8
gtsam/slam/PartialPriorFactor.h
View File

@ -118,16 +118,16 @@ namespace gtsam {
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = zeros(this->dim(), p.dim()); if (H) (*H) = zeros(this->dim(), p.dim());
Vector full_logmap = T::Logmap(p); Vector full_unretraction = T::Unretract(p);
Vector masked_logmap = zero(this->dim()); Vector masked_unretraction = zero(this->dim());
size_t masked_idx=0; size_t masked_idx=0;
for (size_t i=0;i<mask_.size();++i) for (size_t i=0;i<mask_.size();++i)
if (mask_[i]) { if (mask_[i]) {
masked_logmap(masked_idx) = full_logmap(i); masked_unretraction(masked_idx) = full_unretraction(i);
if (H) (*H)(masked_idx, i) = 1.0; if (H) (*H)(masked_idx, i) = 1.0;
++masked_idx; ++masked_idx;
} }
return masked_logmap - prior_; return masked_unretraction - prior_;
} }
// access // access

2
gtsam/slam/PriorFactor.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = eye(p.dim()); if (H) (*H) = eye(p.dim());
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.logmap(p); return prior_.unretract(p);
} }
private: private:

2
gtsam/slam/dataset.cpp
View File

@ -130,7 +130,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
} }
if (addNoise) if (addNoise)
l1Xl2 = l1Xl2.expmap((*model)->sample()); l1Xl2 = l1Xl2.retract((*model)->sample());
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
if (!poses->exists(id1)) poses->insert(id1, Pose2()); if (!poses->exists(id1)) poses->insert(id1, Pose2());

4
gtsam/slam/simulated2DOriented.h
View File

@ -69,7 +69,7 @@ namespace gtsam {
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
return z_.logmap(prior(x, H)); return z_.unretract(prior(x, H));
} }
}; };
@ -93,7 +93,7 @@ namespace gtsam {
Vector evaluateError(const Pose2& x1, const Pose2& x2, Vector evaluateError(const Pose2& x1, const Pose2& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
return z_.logmap(odo(x1, x2, H1, H2)); return z_.unretract(odo(x1, x2, H1, H2));
} }
}; };

2
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -333,7 +333,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
skew_noise, // s skew_noise, // s
trans_noise, trans_noise // ux, uy trans_noise, trans_noise // ux, uy
) ; ) ;
values->insert((int)i, X[i].expmap(delta)) ; values->insert((int)i, X[i].retract(delta)) ;
} }
} }

2
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -327,7 +327,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
trans_noise, trans_noise, trans_noise, // translation trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2 focal_noise, distort_noise, distort_noise // f, k1, k2
) ; ) ;
values->insert((int)i, X[i].expmap(delta)) ; values->insert((int)i, X[i].retract(delta)) ;
} }
} }

20
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -146,8 +146,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
// Create initial config // Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values()); boost::shared_ptr<Pose2Values> initial(new Pose2Values());
initial->insert(0, p0); initial->insert(0, p0);
initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].expmap(Vector_(3, 0.1,-0.1, 0.1))); initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering // Choose an ordering
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
@ -186,11 +186,11 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
// Create initial config // Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values()); boost::shared_ptr<Pose2Values> initial(new Pose2Values());
initial->insert(0, p0); initial->insert(0, p0);
initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].expmap(Vector_(3, 0.1,-0.1, 0.1))); initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(3, hexagon[3].expmap(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].expmap(Vector_(3, 0.1,-0.1, 0.1))); initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].expmap(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering // Choose an ordering
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
@ -327,7 +327,7 @@ TEST( Pose2Values, expmap )
delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0); delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0); delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0); delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0);
Pose2Values actual = circle.expmap(delta, ordering); Pose2Values actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
@ -362,7 +362,7 @@ TEST( Pose2Prior, error )
VectorValues addition(VectorValues::Zero(x0.dims(ordering))); VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
VectorValues plus = delta + addition; VectorValues plus = delta + addition;
Pose2Values x1 = x0.expmap(plus, ordering); Pose2Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -428,7 +428,7 @@ TEST( Pose2Factor, error )
// Check error after increasing p2 // Check error after increasing p2
VectorValues plus = delta; VectorValues plus = delta;
plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
Pose2Values x1 = x0.expmap(plus, ordering); Pose2Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));

16
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -69,11 +69,11 @@ TEST(Pose3Graph, optimizeCircle) {
// Create initial config // Create initial config
boost::shared_ptr<Pose3Values> initial(new Pose3Values()); boost::shared_ptr<Pose3Values> initial(new Pose3Values());
initial->insert(0, gT0); initial->insert(0, gT0);
initial->insert(1, hexagon[1].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].expmap(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(3, hexagon[3].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].expmap(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize // Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
@ -140,9 +140,9 @@ TEST( Pose3Factor, error )
x.insert(1,t1); x.insert(1,t1);
x.insert(2,t2); x.insert(2,t2);
// Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2)) // Get error h(x)-z -> unretract(z,h(x)) = unretract(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x); Vector actual = factor.unwhitenedError(x);
Vector expected = z.logmap(t1.between(t2)); Vector expected = z.unretract(t1.between(t2));
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
@ -223,7 +223,7 @@ TEST( Pose3Values, expmap )
0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0); 0.0,0.0,0.0, 0.1, 0.0, 0.0);
Pose3Values actual = pose3SLAM::circle(4,1.0).expmap(delta, ordering); Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

2
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -87,7 +87,7 @@ TEST( ProjectionFactor, error )
VectorValues delta(expected_config.dims(ordering)); VectorValues delta(expected_config.dims(ordering));
delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.); delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering["l1"]] = Vector_(3, 1.,2.,3.); delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
Values actual_config = config.expmap(delta, ordering); Values actual_config = config.retract(delta, ordering);
CHECK(assert_equal(expected_config,actual_config,1e-9)); CHECK(assert_equal(expected_config,actual_config,1e-9));
} }

2
gtsam/slam/tests/testVSLAM.cpp
View File

@ -210,7 +210,7 @@ TEST( Values, update_with_large_delta) {
delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1); delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
Values actual = init.expmap(delta, largeOrdering); Values actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

2
tests/testExtendedKalmanFilter.cpp
View File

@ -232,7 +232,7 @@ public:
} }
// Return the error between the prediction and the supplied value of p2 // Return the error between the prediction and the supplied value of p2
return prediction.logmap(p2); return prediction.unretract(p2);
} }
}; };

2
tests/testGaussianISAM2.cpp
View File

@ -186,7 +186,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
// gbn.print("Expected bayesnet: "); // gbn.print("Expected bayesnet: ");
VectorValues delta = optimize(gbn); VectorValues delta = optimize(gbn);
planarSLAM::Values expected = fullinit.expmap(delta, ordering); planarSLAM::Values expected = fullinit.retract(delta, ordering);
return assert_equal(expected, actual); return assert_equal(expected, actual);
} }

4
tests/testGaussianJunctionTree.cpp
View File

@ -179,11 +179,11 @@ TEST(GaussianJunctionTree, slamlike) {
GaussianJunctionTree gjt(linearized); GaussianJunctionTree gjt(linearized);
VectorValues deltaactual = gjt.optimize(&EliminateQR); VectorValues deltaactual = gjt.optimize(&EliminateQR);
planarSLAM::Values actual = init.expmap(deltaactual, ordering); planarSLAM::Values actual = init.retract(deltaactual, ordering);
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
VectorValues delta = optimize(gbn); VectorValues delta = optimize(gbn);
planarSLAM::Values expected = init.expmap(delta, ordering); planarSLAM::Values expected = init.retract(delta, ordering);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }

2
tests/testNonlinearISAM.cpp
View File

@ -61,7 +61,7 @@ TEST(testNonlinearISAM, markov_chain ) {
} }
cur_pose = cur_pose.compose(z); cur_pose = cur_pose.compose(z);
new_init.insert(key2, cur_pose.expmap(sampler.sample())); new_init.insert(key2, cur_pose.retract(sampler.sample()));
expected.insert(key2, cur_pose); expected.insert(key2, cur_pose);
isam.update(new_factors, new_init); isam.update(new_factors, new_init);
} }

24
tests/testTupleValues.cpp
View File

@ -193,17 +193,17 @@ TEST(TupleValues, zero_expmap_logmap)
delta[o["l2"]] = Vector_(2, 1.3, 1.4); delta[o["l2"]] = Vector_(2, 1.3, 1.4);
Values expected; Values expected;
expected.insert(PoseKey(1), x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); expected.insert(PoseKey(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(PoseKey(2), x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(PoseKey(2), x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(PointKey(1), Point2(5.0, 6.1)); expected.insert(PointKey(1), Point2(5.0, 6.1));
expected.insert(PointKey(2), Point2(10.3, 11.4)); expected.insert(PointKey(2), Point2(10.3, 11.4));
Values actual = values1.expmap(delta, o); Values actual = values1.retract(delta, o);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// Check log // Check log
VectorValues expected_log = delta; VectorValues expected_log = delta;
VectorValues actual_log = values1.logmap(actual, o); VectorValues actual_log = values1.unretract(actual, o);
CHECK(assert_equal(expected_log, actual_log)); CHECK(assert_equal(expected_log, actual_log));
} }
@ -454,13 +454,13 @@ TEST(TupleValues, expmap)
delta[o["l2"]] = Vector_(2, 1.3, 1.4); delta[o["l2"]] = Vector_(2, 1.3, 1.4);
ValuesA expected; ValuesA expected;
expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(l1k, Point2(5.0, 6.1)); expected.insert(l1k, Point2(5.0, 6.1));
expected.insert(l2k, Point2(10.3, 11.4)); expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, values1.expmap(delta, o))); CHECK(assert_equal(expected, values1.retract(delta, o)));
CHECK(assert_equal(delta, values1.logmap(expected, o))); CHECK(assert_equal(delta, values1.unretract(expected, o)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -485,13 +485,13 @@ TEST(TupleValues, expmap_typedefs)
delta[o["l1"]] = Vector_(2, 1.0, 1.1); delta[o["l1"]] = Vector_(2, 1.0, 1.1);
delta[o["l2"]] = Vector_(2, 1.3, 1.4); delta[o["l2"]] = Vector_(2, 1.3, 1.4);
expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(l1k, Point2(5.0, 6.1)); expected.insert(l1k, Point2(5.0, 6.1));
expected.insert(l2k, Point2(10.3, 11.4)); expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, TupleValues2<PoseValues, PointValues>(values1.expmap(delta, o)))); CHECK(assert_equal(expected, TupleValues2<PoseValues, PointValues>(values1.retract(delta, o))));
//CHECK(assert_equal(delta, values1.logmap(expected))); //CHECK(assert_equal(delta, values1.unretract(expected)));
} }
/* ************************************************************************* */ /* ************************************************************************* */