diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 6a596ce26..68ffac912 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -69,12 +69,6 @@ namespace gtsam { /// Return canonical coordinate 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 inline size_t dim() const { return 6 ; } diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h index 026eeeb87..5dbc28d0f 100644 --- a/gtsam/geometry/CalibratedCameraT.h +++ b/gtsam/geometry/CalibratedCameraT.h @@ -50,11 +50,11 @@ namespace gtsam { return CalibratedCameraT( pose_.inverse(), k_ ) ; } - CalibratedCameraT expmap(const Vector& d) const { - return CalibratedCameraT(pose().expmap(d), k_) ; + CalibratedCameraT retract(const Vector& d) const { + return CalibratedCameraT(pose().retract(d), k_) ; } - Vector logmap(const CalibratedCameraT& T2) const { - return pose().logmap(T2.pose()) ; + Vector unretract(const CalibratedCameraT& T2) const { + return pose().unretract(T2.pose()) ; } void print(const std::string& s = "") const { diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index 6ec87e0d8..01382e9b2 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -116,31 +116,28 @@ class GeneralCameraT { calibration_.print(s + ".calibration.") ; } - GeneralCameraT expmap(const Vector &v) const { + GeneralCameraT retract(const Vector &v) const { Vector v1 = sub(v,0,Camera::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 { - //std::cout << "logmap" << std::endl; - const Vector v1(calibrated().logmap(C.calibrated())), - v2(calibration().logmap(C.calibration())); + Vector unretract(const GeneralCameraT &C) const { + const Vector v1(calibrated().unretract(C.calibrated())), + v2(calibration().unretract(C.calibration())); return concatVectors(2,&v1,&v2) ; } - static GeneralCameraT Expmap(const Vector& v) { - //std::cout << "Expmap" << std::endl; + static GeneralCameraT Retract(const Vector& v) { return GeneralCameraT( - Camera::Expmap(sub(v,0,Camera::Dim())), - Calibration::Expmap(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim())) + Camera::Retract(sub(v,0,Camera::Dim())), + Calibration::Retract(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim())) ); } - static Vector Logmap(const GeneralCameraT& p) { - //std::cout << "Logmap" << std::endl; - const Vector v1(Camera::Logmap(p.calibrated())), - v2(Calibration::Logmap(p.calibration())); + static Vector Unretract(const GeneralCameraT& p) { + const Vector v1(Camera::Unretract(p.calibrated())), + v2(Calibration::Unretract(p.calibration())); return concatVectors(2,&v1,&v2); } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index aa139eaa3..823853f6d 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -43,7 +43,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 from the Bayes Network 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. // This will act as a prior for the next iteration. diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index d94865cf2..d32085eeb 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -237,7 +237,7 @@ struct _SelectiveExpmapAndClear { assert(delta[var].size() == (int)it_x->second.dim()); assert(delta[var].unaryExpr(&isfinite).all()); if(mask[var]) { - it_x->second = it_x->second.expmap(delta[var]); + it_x->second = it_x->second.retract(delta[var]); if(invalidate) (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index c62f843e0..4e8accd4e 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -548,7 +548,7 @@ template Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); - return theta_.expmap(delta, ordering_); + return theta_.retract(delta, ordering_); } } diff --git a/gtsam/nonlinear/LieValues-inl.h b/gtsam/nonlinear/LieValues-inl.h index 2e7f2b3f6..18d3b207d 100644 --- a/gtsam/nonlinear/LieValues-inl.h +++ b/gtsam/nonlinear/LieValues-inl.h @@ -136,7 +136,7 @@ namespace gtsam { /* ************************************************************************* */ // todo: insert for every element is inefficient template - LieValues LieValues::expmap(const VectorValues& delta, const Ordering& ordering) const { + LieValues LieValues::retract(const VectorValues& delta, const Ordering& ordering) const { LieValues newValues; typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, this->values_) { @@ -144,7 +144,7 @@ namespace gtsam { const typename J::Value& pj = value.second; Index index; if(ordering.tryAt(j,index)) { - newValues.insert(j, pj.expmap(delta[index])); + newValues.insert(j, pj.retract(delta[index])); } else newValues.insert(j, pj); } @@ -171,7 +171,7 @@ namespace gtsam { // /* ************************************************************************* */ // // todo: insert for every element is inefficient // template -// LieValues LieValues::expmap(const Vector& delta) const { +// LieValues LieValues::retract(const Vector& delta) const { // if(delta.size() != dim()) { // cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // throw invalid_argument("Delta vector length does not match config dimensionality."); @@ -183,7 +183,7 @@ namespace gtsam { // const J& j = value.first; // const typename J::Value& pj = value.second; // 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; // } // return newValues; @@ -193,19 +193,19 @@ namespace gtsam { // todo: insert for every element is inefficient // todo: currently only logmaps elements in both configs template - inline VectorValues LieValues::logmap(const LieValues& cp, const Ordering& ordering) const { + inline VectorValues LieValues::unretract(const LieValues& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - logmap(cp, ordering, delta); + unretract(cp, ordering, delta); return delta; } /* ************************************************************************* */ template - void LieValues::logmap(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const { + void LieValues::unretract(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const { typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, cp) { 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); } } diff --git a/gtsam/nonlinear/LieValues.h b/gtsam/nonlinear/LieValues.h index 97bec1f35..4e6c13cb0 100644 --- a/gtsam/nonlinear/LieValues.h +++ b/gtsam/nonlinear/LieValues.h @@ -119,13 +119,13 @@ namespace gtsam { // Lie operations /** 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) */ - 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) */ - void logmap(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; + void unretract(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; // imperative methods: diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index ce3738ed9..d41c40147 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -573,7 +573,7 @@ public: Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { const size_t p = X::Dim(); if (H1) *H1 = eye(p); - return value_.logmap(x1); + return value_.unretract(x1); } /** Print */ @@ -628,7 +628,7 @@ public: const size_t p = X::Dim(); if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); - return x1.logmap(x2); + return x1.unretract(x2); } private: diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e36e22f1a..f68674dc0 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -120,7 +120,7 @@ namespace gtsam { size_t nj = feasible_.dim(); if (allow_error_) { 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)) { if (H) *H = eye(nj); return zero(nj); // set error to zero if equal diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index eba64cf33..761f2e032 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -93,7 +93,7 @@ void NonlinearISAM::reorder_relinearize() { template Values NonlinearISAM::estimate() const { if(isam_.size() > 0) - return linPoint_.expmap(optimize(isam_), ordering_); + return linPoint_.retract(optimize(isam_), ordering_); else return linPoint_; } diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index c2380c672..5e6d9ebc6 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -89,7 +89,7 @@ namespace gtsam { if (verbosity >= Parameters::DELTA) delta.print("delta"); // 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 if (verbosity >= Parameters::VALUES) newValues->print("newValues"); @@ -179,7 +179,7 @@ namespace gtsam { if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); // 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 double error = graph_->error(*newValues); diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index 977f361d3..e282cc226 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -205,21 +205,21 @@ namespace gtsam { } /** Expmap */ - TupleValues expmap(const VectorValues& delta, const Ordering& ordering) const { - return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering)); + TupleValues retract(const VectorValues& delta, const Ordering& ordering) const { + return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering)); } /** logmap each element */ - VectorValues logmap(const TupleValues& cp, const Ordering& ordering) const { + VectorValues unretract(const TupleValues& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - logmap(cp, ordering, delta); + unretract(cp, ordering, delta); return delta; } /** logmap each element */ - void logmap(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { - first_.logmap(cp.first_, ordering, delta); - second_.logmap(cp.second_, ordering, delta); + void unretract(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { + first_.unretract(cp.first_, ordering, delta); + second_.unretract(cp.second_, ordering, delta); } /** @@ -318,18 +318,18 @@ namespace gtsam { size_t dim() const { return first_.dim(); } - TupleValuesEnd expmap(const VectorValues& delta, const Ordering& ordering) const { - return TupleValuesEnd(first_.expmap(delta, ordering)); + TupleValuesEnd retract(const VectorValues& delta, const Ordering& ordering) const { + return TupleValuesEnd(first_.retract(delta, ordering)); } - VectorValues logmap(const TupleValuesEnd& cp, const Ordering& ordering) const { + VectorValues unretract(const TupleValuesEnd& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - logmap(cp, ordering, delta); + unretract(cp, ordering, delta); return delta; } - void logmap(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { - first_.logmap(cp.first_, ordering, delta); + void unretract(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { + first_.unretract(cp.first_, ordering, delta); } /** diff --git a/gtsam/nonlinear/tests/testLieValues.cpp b/gtsam/nonlinear/tests/testLieValues.cpp index a4e0cc698..9a0b39ffc 100644 --- a/gtsam/nonlinear/tests/testLieValues.cpp +++ b/gtsam/nonlinear/tests/testLieValues.cpp @@ -137,7 +137,7 @@ TEST(LieValues, expmap_a) expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); 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(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(key2, Vector_(3, 6.3, 7.4, 8.5)); // -// CHECK(assert_equal(expected, config0.expmap(increment))); +// CHECK(assert_equal(expected, config0.retract(increment))); //} /* ************************************************************************* */ diff --git a/gtsam/slam/BetweenConstraint.h b/gtsam/slam/BetweenConstraint.h index 0d4ae632c..33b1ab524 100644 --- a/gtsam/slam/BetweenConstraint.h +++ b/gtsam/slam/BetweenConstraint.h @@ -51,7 +51,7 @@ namespace gtsam { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { X hx = x1.between(x2, H1, H2); - return measured_.logmap(hx); + return measured_.unretract(hx); } inline const X measured() const { diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 339dbad3b..c8fddbea0 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -83,7 +83,7 @@ namespace gtsam { boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.logmap(hx); + return measured_.unretract(hx); } /** return the measured */ diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 37f084033..7e728370c 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -82,7 +82,7 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional 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"); return error; } diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 9b76ee5bc..08edc7dc8 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -118,16 +118,16 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { if (H) (*H) = zeros(this->dim(), p.dim()); - Vector full_logmap = T::Logmap(p); - Vector masked_logmap = zero(this->dim()); + Vector full_unretraction = T::Unretract(p); + Vector masked_unretraction = zero(this->dim()); size_t masked_idx=0; for (size_t i=0;i H = boost::none) const { if (H) (*H) = eye(p.dim()); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.logmap(p); + return prior_.unretract(p); } private: diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 6b247c3fe..b5322ee47 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -130,7 +130,7 @@ pair load2D(const string& filename, } if (addNoise) - l1Xl2 = l1Xl2.expmap((*model)->sample()); + l1Xl2 = l1Xl2.retract((*model)->sample()); // Insert vertices if pure odometry file if (!poses->exists(id1)) poses->insert(id1, Pose2()); diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 0934ed6b0..08dc30e43 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -69,7 +69,7 @@ namespace gtsam { /// Evaluate error and optionally derivative Vector evaluateError(const Pose2& x, boost::optional H = 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, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return z_.logmap(odo(x1, x2, H1, H2)); + return z_.unretract(odo(x1, x2, H1, H2)); } }; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 096e0d13b..85cf0159e 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -333,7 +333,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { skew_noise, // s trans_noise, trans_noise // ux, uy ) ; - values->insert((int)i, X[i].expmap(delta)) ; + values->insert((int)i, X[i].retract(delta)) ; } } diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index afc5fa526..c0d80671d 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -327,7 +327,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { trans_noise, trans_noise, trans_noise, // translation 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)) ; } } diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index f938d055f..7b6e0dd61 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -146,8 +146,8 @@ TEST(Pose2Graph, optimizeThreePoses) { // Create initial config boost::shared_ptr initial(new Pose2Values()); initial->insert(0, p0); - initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].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].retract(Vector_(3, 0.1,-0.1, 0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -186,11 +186,11 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { // Create initial config boost::shared_ptr initial(new Pose2Values()); initial->insert(0, p0); - initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].expmap(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(3, hexagon[3].expmap(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(4, hexagon[4].expmap(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(5, hexagon[5].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].retract(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].retract(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 shared_ptr 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(2)]] = Vector_(3, 0.0,0.1,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)); } @@ -362,7 +362,7 @@ TEST( Pose2Prior, error ) VectorValues addition(VectorValues::Zero(x0.dims(ordering))); addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); 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 ! CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); @@ -428,7 +428,7 @@ TEST( Pose2Factor, error ) // Check error after increasing p2 VectorValues plus = delta; 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 ! CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 0efd2c5f5..7049baaf9 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -69,11 +69,11 @@ TEST(Pose3Graph, optimizeCircle) { // Create initial config boost::shared_ptr initial(new Pose3Values()); 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(2, hexagon[2].expmap(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(4, hexagon[4].expmap(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(1, hexagon[1].retract(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].retract(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].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize shared_ptr ordering(new Ordering); @@ -140,9 +140,9 @@ TEST( Pose3Factor, error ) x.insert(1,t1); 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 expected = z.logmap(t1.between(t2)); + Vector expected = z.unretract(t1.between(t2)); 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); - 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)); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 29cf4842e..5ae357ef6 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -87,7 +87,7 @@ TEST( ProjectionFactor, error ) VectorValues delta(expected_config.dims(ordering)); delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.); 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)); } diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index 9651ee4e1..1baa1642b 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -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["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); - Values actual = init.expmap(delta, largeOrdering); + Values actual = init.retract(delta, largeOrdering); CHECK(assert_equal(expected,actual)); } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 50723fbe7..2ea5cf12f 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -232,7 +232,7 @@ public: } // Return the error between the prediction and the supplied value of p2 - return prediction.logmap(p2); + return prediction.unretract(p2); } }; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 1940a151b..ad9855fa5 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -186,7 +186,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); // gbn.print("Expected bayesnet: "); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = fullinit.expmap(delta, ordering); + planarSLAM::Values expected = fullinit.retract(delta, ordering); return assert_equal(expected, actual); } diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 2e7e98a2e..4a849da1d 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -179,11 +179,11 @@ TEST(GaussianJunctionTree, slamlike) { GaussianJunctionTree gjt(linearized); VectorValues deltaactual = gjt.optimize(&EliminateQR); - planarSLAM::Values actual = init.expmap(deltaactual, ordering); + planarSLAM::Values actual = init.retract(deltaactual, ordering); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = init.expmap(delta, ordering); + planarSLAM::Values expected = init.retract(delta, ordering); EXPECT(assert_equal(expected, actual)); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index b37631d5f..31edc5150 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -61,7 +61,7 @@ TEST(testNonlinearISAM, markov_chain ) { } 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); isam.update(new_factors, new_init); } diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp index de83e1540..f95013421 100644 --- a/tests/testTupleValues.cpp +++ b/tests/testTupleValues.cpp @@ -193,17 +193,17 @@ TEST(TupleValues, zero_expmap_logmap) delta[o["l2"]] = Vector_(2, 1.3, 1.4); Values expected; - expected.insert(PoseKey(1), x1.expmap(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(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2))); + 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(2), Point2(10.3, 11.4)); - Values actual = values1.expmap(delta, o); + Values actual = values1.retract(delta, o); CHECK(assert_equal(expected, actual)); // Check log 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)); } @@ -454,13 +454,13 @@ TEST(TupleValues, expmap) delta[o["l2"]] = Vector_(2, 1.3, 1.4); ValuesA expected; - expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); + expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); + expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(l1k, Point2(5.0, 6.1)); expected.insert(l2k, Point2(10.3, 11.4)); - CHECK(assert_equal(expected, values1.expmap(delta, o))); - CHECK(assert_equal(delta, values1.logmap(expected, o))); + CHECK(assert_equal(expected, values1.retract(delta, 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["l2"]] = Vector_(2, 1.3, 1.4); - expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); + expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); + expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(l1k, Point2(5.0, 6.1)); expected.insert(l2k, Point2(10.3, 11.4)); - CHECK(assert_equal(expected, TupleValues2(values1.expmap(delta, o)))); - //CHECK(assert_equal(delta, values1.logmap(expected))); + CHECK(assert_equal(expected, TupleValues2(values1.retract(delta, o)))); + //CHECK(assert_equal(delta, values1.unretract(expected))); } /* ************************************************************************* */