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

Updates to dogleg to work with newer Lie/Manifold/Group interface

release/4.3a0
Alex Cunningham 2011-11-05 23:01:50 +00:00
parent 1ec7d7e86e
commit 4284f07a61
5 changed files with 130 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Pose3.cpp
View File

@ -109,7 +109,7 @@ namespace gtsam {
/* ************************************************************************* */
Pose3 retract(const Vector& d) {
return expmap(d);
return retract(d);
}
/* ************************************************************************* */

22
gtsam/geometry/Pose3.h
View File

@ -23,23 +23,23 @@
namespace gtsam {
/**
/**
* A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry
*/
class Pose3 {
public:
class Pose3 {
public:
static const size_t dimension = 6;
/** Pose Concept requirements */
typedef Rot3 Rotation;
typedef Point3 Translation;
private:
private:
Rot3 R_;
Point3 t_;
public:
public:
/** Default constructor is origin */
Pose3() {}
@ -178,7 +178,7 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
@ -186,18 +186,18 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
}; // Pose3 class
}; // Pose3 class
/**
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = 3D angular velocity
* v = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
template <>
inline Matrix wedge<Pose3>(const Vector& xi) {
template <>
inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
}
}
} // namespace gtsam

2
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -159,7 +159,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl;
// Compute expmapped solution
const VALUES x_d(x0.expmap(result.dx_d, ordering));
const VALUES x_d(x0.retract(result.dx_d, ordering));
// Compute decrease in f
result.f_error = f.error(x_d);

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

@ -306,7 +306,7 @@ namespace gtsam {
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
*graph_, *values_, *ordering_, error_);
shared_values newValues(new T(values_->expmap(result.dx_d, *ordering_)));
shared_values newValues(new T(values_->retract(result.dx_d, *ordering_)));
cout << "newValues: " << newValues.get() << endl;
return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
}

2
tests/testDoglegOptimizer.cpp
View File

@ -405,7 +405,7 @@ TEST(DoglegOptimizer, Iterate) {
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config));
Delta = result.Delta;
EXPECT(result.f_error < fg->error(*config)); // Check that error decreases
simulated2D::Values newConfig(config->expmap(result.dx_d, *ord));
simulated2D::Values newConfig(config->retract(result.dx_d, *ord));
(*config) = newConfig;
DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in
}