27 #include <gtsam/dllexport.h> 69 r_(
Rot2::fromAngle(theta)), t_(x, y) {
74 r_(
Rot2::fromAngle(theta)), t_(t) {
82 r_(
Rot2::
atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
83 assert(T.rows() == 3 && T.cols() == 3);
100 GTSAM_EXPORT
void print(
const std::string&
s =
"")
const;
117 return Pose2(r_*p2.
r(), t_ + r_*p2.
t());
171 static inline Matrix3
wedge(
double vx,
double vy,
double w) {
215 inline double x()
const {
return t_.x(); }
218 inline double y()
const {
return t_.y(); }
236 GTSAM_EXPORT Matrix3
matrix()
const;
299 friend class boost::serialization::access;
300 template<
class Archive>
302 ar & BOOST_SERIALIZATION_NVP(t_);
303 ar & BOOST_SERIALIZATION_NVP(r_);
323 GTSAM_EXPORT boost::optional<Pose2>
align(
const std::vector<Point2Pair>& pairs);
332 template <
typename T>
335 template <
typename T>
GTSAM_EXPORT Matrix3 matrix() const
std::pair< Point2, Point2 > Point2Pair
const Point2 & t() const
translation
void serialize(Archive &ar, const unsigned int)
const Point2 & translation() const
translation
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Pose2(double x, double y, double theta)
static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
static Matrix3 wedge(double vx, double vy, double w)
const Rot2 & r() const
rotation
GTSAM_EXPORT void print(const std::string &s="") const
static std::pair< size_t, size_t > translationInterval()
GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Point2 operator*(const Point2 &point) const
StridedVectorType vy(make_vector(y,*n, std::abs(*incy)))
static Matrix3 adjointMap_(const Vector3 &xi)
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
double theta() const
get theta
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
const Rot2 & rotation() const
rotation
Matrix wedge< Pose2 >(const Vector &xi)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Pose2(const Rot2 &r, const Point2 &t)
Base class and basic functions for Lie types.
Jet< T, N > atan2(const Jet< T, N > &g, const Jet< T, N > &f)
static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
Derivative of Logmap.
boost::optional< Pose2 > align(const vector< Point2Pair > &pairs)
static std::pair< size_t, size_t > rotationInterval()
static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y)
static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v)
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y)
GTSAM_EXPORT Matrix3 AdjointMap() const
ofstream os("timeSchurFactors.csv")
Both LieGroupTraits and Testable.
Pose2(double theta, const Point2 &t)
static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
Output stream operator.
StridedVectorType vx(make_vector(x,*n, std::abs(*incx)))
GTSAM_EXPORT Pose2 inverse() const
inverse
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
static GTSAM_EXPORT Pose2 Retract(const Vector3 &v, ChartJacobian H=boost::none)
static Vector3 adjoint(const Vector3 &xi, const Vector3 &y)
GTSAM_EXPORT bool equals(const Pose2 &pose, double tol=1e-9) const
GTSAM_EXPORT double range(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
static GTSAM_EXPORT Vector3 Local(const Pose2 &r, ChartJacobian H=boost::none)
static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
Derivative of Expmap.
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
static Pose2 identity()
identity for group operation