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) { Pose3 retract(const Vector& d) {
return expmap(d); return retract(d);
} }
/* ************************************************************************* */ /* ************************************************************************* */

252
gtsam/geometry/Pose3.h
View File

@ -23,142 +23,142 @@
namespace gtsam { namespace gtsam {
/** /**
* A 3D pose (R,t) : (Rot3,Point3) * A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry * @ingroup geometry
*/ */
class Pose3 { class Pose3 {
public: public:
static const size_t dimension = 6; static const size_t dimension = 6;
/** Pose Concept requirements */ /** Pose Concept requirements */
typedef Rot3 Rotation; typedef Rot3 Rotation;
typedef Point3 Translation; typedef Point3 Translation;
private: private:
Rot3 R_; Rot3 R_;
Point3 t_; Point3 t_;
public: public:
/** Default constructor is origin */ /** Default constructor is origin */
Pose3() {} Pose3() {}
/** Copy constructor */ /** Copy constructor */
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
/** Construct from R,t */ /** Construct from R,t */
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
/** Constructor from 4*4 matrix */ /** Constructor from 4*4 matrix */
Pose3(const Matrix &T) : Pose3(const Matrix &T) :
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {}
/** Constructor from 12D vector */ /** Constructor from 12D vector */
Pose3(const Vector &V) : Pose3(const Vector &V) :
R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)),
t_(V(9), V(10),V(11)) {} t_(V(9), V(10),V(11)) {}
inline const Rot3& rotation() const { return R_; } inline const Rot3& rotation() const { return R_; }
inline const Point3& translation() const { return t_; } inline const Point3& translation() const { return t_; }
inline double x() const { return t_.x(); } inline double x() const { return t_.x(); }
inline double y() const { return t_.y(); } inline double y() const { return t_.y(); }
inline double z() const { return t_.z(); } inline double z() const { return t_.z(); }
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix matrix() const; Matrix matrix() const;
/** print with optional string */ /** print with optional string */
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
/** assert equality up to a tolerance */ /** assert equality up to a tolerance */
bool equals(const Pose3& pose, double tol = 1e-9) const; bool equals(const Pose3& pose, double tol = 1e-9) const;
/** Compose two poses */ /** Compose two poses */
inline Pose3 operator*(const Pose3& T) const { inline Pose3 operator*(const Pose3& T) const {
return Pose3(R_*T.R_, t_ + R_*T.t_); return Pose3(R_*T.R_, t_ + R_*T.t_);
} }
Pose3 transform_to(const Pose3& pose) const; Pose3 transform_to(const Pose3& pose) const;
/** dimension of the variable - used to autodetect sizes */ /** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; } inline static size_t Dim() { return dimension; }
/** Lie requirements */ /** Lie requirements */
/** Dimensionality of the tangent space */ /** Dimensionality of the tangent space */
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** identity */ /** identity */
inline static Pose3 identity() { inline static Pose3 identity() {
return Pose3(); return Pose3();
} }
/** /**
* Derivative of inverse * Derivative of inverse
*/ */
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const; Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/** /**
* composes two poses (first (*this) then p2) * composes two poses (first (*this) then p2)
* with optional derivatives * with optional derivatives
*/ */
Pose3 compose(const Pose3& p2, Pose3 compose(const Pose3& p2,
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;
/** receives the point in Pose coordinates and transforms it to world coordinates */ /** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Point3& p, Point3 transform_from(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** syntactic sugar for transform */ /** syntactic sugar for transform */
inline Point3 operator*(const Point3& p) const { return transform_from(p); } inline Point3 operator*(const Point3& p) const { return transform_from(p); }
/** receives the point in world coordinates and transforms it to Pose coordinates */ /** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map around another pose */ /** Exponential map around another pose */
Pose3 retract(const Vector& d) const; Pose3 retract(const Vector& d) const;
/** Logarithm map around another pose T1 */ /** Logarithm map around another pose T1 */
Vector localCoordinates(const Pose3& T2) const; Vector localCoordinates(const Pose3& T2) const;
/** non-approximated versions of Expmap/Logmap */ /** non-approximated versions of Expmap/Logmap */
static Pose3 Expmap(const Vector& xi); static Pose3 Expmap(const Vector& xi);
static Vector Logmap(const Pose3& p); static Vector Logmap(const Pose3& p);
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives * as well as optionally the derivatives
*/ */
Pose3 between(const Pose3& p2, Pose3 between(const Pose3& p2,
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;
/** /**
* Calculate Adjoint map * Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
*/ */
Matrix AdjointMap() const; Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
/** /**
* wedge for Pose3: * wedge for Pose3:
* @param xi 6-dim twist (omega,v) where * @param xi 6-dim twist (omega,v) where
* omega = (wx,wy,wz) 3D angular velocity * omega = (wx,wy,wz) 3D angular velocity
* v (vx,vy,vz) = 3D velocity * v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated * @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/ */
static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
return Matrix_(4,4, return Matrix_(4,4,
0.,-wz, wy, vx, 0.,-wz, wy, vx,
wz, 0.,-wx, vy, wz, 0.,-wx, vy,
-wy, wx, 0., vz, -wy, wx, 0., vz,
0., 0., 0., 0.); 0., 0., 0., 0.);
} }
/** /**
* Calculate range to a landmark * Calculate range to a landmark
@ -178,26 +178,26 @@ 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;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(t_);
} }
}; // Pose3 class }; // Pose3 class
/** /**
* wedge for Pose3: * wedge for Pose3:
* @param xi 6-dim twist (omega,v) where * @param xi 6-dim twist (omega,v) where
* omega = 3D angular velocity * omega = 3D angular velocity
* v = 3D velocity * v = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated * @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/ */
template <> template <>
inline Matrix wedge<Pose3>(const Vector& xi) { inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
} }
} // namespace gtsam } // 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; cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl;
// Compute expmapped solution // 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 // Compute decrease in f
result.f_error = f.error(x_d); 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( DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
*graph_, *values_, *ordering_, error_); *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; cout << "newValues: " << newValues.get() << endl;
return newValuesErrorLambda_(newValues, result.f_error, result.Delta); 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)); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config));
Delta = result.Delta; Delta = result.Delta;
EXPECT(result.f_error < fg->error(*config)); // Check that error decreases 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; (*config) = newConfig;
DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in
} }