Go to the documentation of this file.
34 for (
const auto& [
a,
b] : abPointPairs) {
35 Point2 da =
a - centroids.first;
36 Point2 db =
b - centroids.second;
37 d_abPointPairs.emplace_back(da, db);
39 return d_abPointPairs;
47 for (
const auto& [da, db] : d_abPointPairs) {
49 y += da.transpose() * da_prime;
50 x += da_prime.transpose() * da_prime;
52 const double s =
y /
x;
59 for (
const auto& [da, db] : d_abPointPairs) {
60 H += da * db.transpose();
79 const Point2 aTb = (centroids.first -
s * (
aRb * centroids.second)) /
s;
96 auto centroids =
means(abPointPairs);
107 : R_(
R), t_(
t), s_(
s) {}
110 : R_(
Rot2::ClosestTo(
R)), t_(
t), s_(
s) {}
128 std::cout << std::endl;
165 if (abPointPairs.size() < 2)
166 throw std::runtime_error(
"input should have at least 2 pairs of points");
167 auto centroids =
means(abPointPairs);
176 const size_t n = abPosePairs.size();
178 throw std::runtime_error(
"input should have at least 2 pairs of poses");
181 vector<Rot2> rotations;
183 rotations.reserve(
n);
184 abPointPairs.reserve(
n);
187 for (
const auto& [aTi, bTi] : abPosePairs) {
188 const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
189 rotations.emplace_back(
aRb);
190 abPointPairs.emplace_back(aTi.translation(), bTi.translation());
192 const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
201 const double s =
log(
S.s_);
205 throw std::runtime_error(
"Similarity2::Logmap: derivative not implemented");
214 const double s =
v[3];
216 throw std::runtime_error(
"Similarity2::Expmap: derivative not implemented");
222 throw std::runtime_error(
"Similarity2::AdjointMap not implemented");
226 os <<
"[" <<
p.rotation().theta() <<
" " <<
p.translation().transpose() <<
" "
227 <<
p.scale() <<
"]\';";
234 T.bottomRows<1>() << 0, 0, 1.0 /
s_;
Similarity2()
Default constructor.
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
static Similarity2 Expmap(const Vector4 &v, OptionalJacobian< 4, 4 > Hm={})
Exponential map at the identity.
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
bool operator==(const Similarity2 &other) const
Exact equality.
Class compose(const Class &g) const
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 x
Implementation of Similarity2 transform.
Eigen::Triplet< double > T
const EIGEN_DEVICE_FUNC LogReturnType log() const
ofstream os("timeSchurFactors.csv")
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
mat1 topRightCorner(size/2, size/2)
3D rotation represented as a rotation matrix or quaternion
static Similarity2 Identity()
Return an identity transform.
std::pair< Point2, Point2 > Point2Pair
static Matrix2 CalculateH(const Point2Pairs &d_abPointPairs)
Form outer product H.
static Similarity2 AlignGivenR(const Point2Pairs &abPointPairs, const Rot2 &aRb)
This method estimates the similarity transform from point pairs, given a known or estimated rotation....
bool equals(const Rot2 &R, double tol=1e-9) const
EIGEN_DEVICE_FUNC const Scalar & q
void print(const std::string &s="") const
Print with optional string.
static Vector1 Logmap(const Rot2 &r, ChartJacobian H={})
Log map at identity - return the canonical coordinates of this rotation.
Eigen::Block< Derived > topLeftCorner(MatrixBase< Derived > &m, int rows, int cols)
Base class and basic functions for Manifold types.
void print(const std::string &s="theta") const
static Point2Pairs SubtractCentroids(const Point2Pairs &abPointPairs, const Point2Pair ¢roids)
Subtract centroids from point pairs.
Rot2 rotation() const
Return a GTSAM rotation.
static Rot2 ClosestTo(const Matrix2 &M)
Matrix4 AdjointMap() const
Project from one tangent space to another.
Matrix3 matrix() const
Calculate 4*4 matrix group equivalent.
static Vector4 Logmap(const Similarity2 &S, OptionalJacobian< 4, 4 > Hm={})
double scale() const
Return the scale.
Point2 transformFrom(const Point2 &p) const
Action on a point p is s*(R*p+t)
Similarity2 inverse() const
Return the inverse.
bool equals(const Similarity2 &sim, double tol) const
Compare with tolerance.
static Similarity2 Align(const Point2Pairs &abPointPairs)
static double CalculateScale(const Point2Pairs &d_abPointPairs, const Rot2 &aRb)
Form inner products x and y and calculate scale.
Array< int, Dynamic, 1 > v
The matrix class, also used for vectors and row-vectors.
std::vector< Point2Pair > Point2Pairs
Point2 translation() const
Return a GTSAM translation.
std::vector< Pose2Pair > Pose2Pairs
static Similarity2 Align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, const Point2Pair ¢roids)
This method estimates the similarity transform from differences point pairs, given a known or estimat...
Rot2 R(Rot2::fromAngle(0.1))
Similarity2 operator*(const Similarity2 &S) const
Composition.
static Rot2 Expmap(const Vector1 &v, ChartJacobian H={})
Exponential map at identity - create a rotation from canonical coordinates.
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:12