Loading...
Searching...
No Matches
Similarity3.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
namespace gtsam {
// Forward declarations
class Pose3;
class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
typedef Rot3 Rotation;
typedef Point3 Translation;
private:
Rot3 R_;
Point3 t_;
double s_;
public:
Similarity3();
Similarity3(double s);
Similarity3(const Rot3& R, const Point3& t, double s);
Similarity3(const Matrix3& R, const Vector3& t, double s);
Similarity3(const Matrix4& T);
bool equals(const Similarity3& sim, double tol) const;
bool operator==(const Similarity3& other) const;
void print(const std::string& s) const;
friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
static Similarity3 Identity();
Similarity3 operator*(const Similarity3& S) const;
Similarity3 inverse() const;
Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
Pose3 transformFrom(const Pose3& T) const;
Point3 operator*(const Point3& p) const;
static Similarity3 Align(const Point3Pairs& abPointPairs);
static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none);
static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none);
struct ChartAtOrigin {
static Similarity3 Retract(const Vector7& v,
ChartJacobian H = boost::none) {
return Similarity3::Expmap(v, H);
}
static Vector7 Local(const Similarity3& other,
ChartJacobian H = boost::none) {
return Similarity3::Logmap(other, H);
}
};
using LieGroup<Similarity3, 7>::inverse;
static Matrix4 wedge(const Vector7& xi);
Matrix7 AdjointMap() const;
Matrix4 matrix() const;
Rot3 rotation() const { return R_; }
Point3 translation() const { return t_; }
double scale() const { return s_; }
inline static size_t Dim() { return 7; }
inline size_t dim() const { return 7; }
private:
static Matrix3 GetV(Vector3 w, double lambda);
};
template <>
inline Matrix wedge<Similarity3>(const Vector& xi) {
return Similarity3::wedge(xi);
}
template <>
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
template <>
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
} // namespace gtsam
Manifold.h
Base class and basic functions for Manifold types.
Lie.h
Base class and basic functions for Lie types.
Pose3.h
3D Pose
Rot3.h
3D rotation represented as a rotation matrix or quaternion
Point3.h
3D Point
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::Similarity3
3D similarity transform
Definition Similarity3.h:36
gtsam::Similarity3::Logmap
static Vector7 Logmap(const Similarity3 &s, OptionalJacobian< 7, 7 > Hm=boost::none)
Log map at the identity .
Definition Similarity3.cpp:256
gtsam::Similarity3::Dim
static size_t Dim()
Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
Definition Similarity3.h:194
gtsam::Similarity3::print
void print(const std::string &s) const
Print with optional string.
Definition Similarity3.cpp:118
gtsam::Similarity3::AdjointMap
Matrix7 AdjointMap() const
Project from one tangent space to another.
Definition Similarity3.cpp:208
gtsam::Similarity3::Expmap
static Similarity3 Expmap(const Vector7 &v, OptionalJacobian< 7, 7 > Hm=boost::none)
Exponential map at the identity.
Definition Similarity3.cpp:269
gtsam::Similarity3::Identity
static Similarity3 Identity()
Return an identity transform.
Definition Similarity3.cpp:125
gtsam::Similarity3::translation
Point3 translation() const
Return a GTSAM translation.
Definition Similarity3.h:188
gtsam::Similarity3::inverse
Similarity3 inverse() const
Return the inverse.
Definition Similarity3.cpp:132
gtsam::Similarity3::Align
static Similarity3 Align(const Point3Pairs &abPointPairs)
Create Similarity3 by aligning at least three point pairs.
Definition Similarity3.cpp:162
gtsam::Similarity3::Similarity3
Similarity3()
Default constructor.
Definition Similarity3.cpp:89
gtsam::Similarity3::rotation
Rot3 rotation() const
Return a GTSAM rotation.
Definition Similarity3.h:185
gtsam::Similarity3::dim
size_t dim() const
Dimensionality of tangent space = 7 DOF.
Definition Similarity3.h:197
gtsam::Similarity3::wedge
static Matrix4 wedge(const Vector7 &xi)
wedge for Similarity3:
Definition Similarity3.cpp:198
gtsam::Similarity3::matrix
Matrix4 matrix() const
Calculate 4*4 matrix group equivalent.
Definition Similarity3.cpp:286
gtsam::Similarity3::equals
bool equals(const Similarity3 &sim, double tol) const
Compare with tolerance.
Definition Similarity3.cpp:109
gtsam::Similarity3::scale
double scale() const
Return the scale.
Definition Similarity3.h:191
gtsam::Similarity3::transformFrom
Point3 transformFrom(const Point3 &p, OptionalJacobian< 3, 7 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Action on a point p is s*(R*p+t).
Definition Similarity3.cpp:138
gtsam::Similarity3::operator*
Similarity3 operator*(const Similarity3 &S) const
Composition.
Definition Similarity3.cpp:128
gtsam::Similarity3::operator==
bool operator==(const Similarity3 &other) const
Exact equality.
Definition Similarity3.cpp:114
gtsam::Similarity3::ChartAtOrigin
Chart at the origin.
Definition Similarity3.h:153