Go to the documentation of this file.
34 for (
const auto& [
a,
b] : abPointPairs) {
35 Point3 da =
a - centroids.first;
36 Point3 db =
b - centroids.second;
37 d_abPointPairs.emplace_back(da, db);
39 return d_abPointPairs;
48 for (
const auto& [da, db] : d_abPointPairs) {
50 y += da.transpose() * da_prime;
51 x += da_prime.transpose() * da_prime;
60 for (
const auto& [da, db] : d_abPointPairs) {
61 H += da * db.transpose();
73 const Point3 aTb = (centroids.first -
s * (
aRb * centroids.second)) /
s;
81 auto centroids =
means(abPointPairs);
83 return align(d_abPointPairs,
aRb, centroids);
100 R_(
R), t_(
t), s_(
s) {
117 std::cout << std::endl;
120 std::cout <<
"t: " <<
translation().transpose() <<
" s: " <<
scale() << std::endl;
143 *H1 << DR, sR, sR *
p;
163 if (abPointPairs.size() < 3)
164 throw std::runtime_error(
"input should have at least 3 pairs of points");
165 auto centroids =
means(abPointPairs);
174 const size_t n = abPosePairs.size();
176 throw std::runtime_error(
"input should have at least 2 pairs of poses");
179 vector<Rot3> rotations;
181 rotations.reserve(
n);
182 abPointPairs.reserve(
n);
185 for (
const auto &[aTi, bTi] : abPosePairs) {
187 rotations.emplace_back(
aRb);
190 const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
197 const auto w =
xi.head<3>();
198 const auto u =
xi.segment<3>(3);
211 adj <<
R, Z_3x3, Matrix31::Zero(),
219 const double theta2 =
w.transpose() *
w;
222 const double theta =
sqrt(theta2);
223 const double X =
sin(theta) / theta;
224 Y = (1 -
cos(theta)) / theta2;
225 Z = (1 -
X) / theta2;
226 W = (0.5 -
Y) / theta2;
229 Y = 0.5 - theta2 / 24.0;
230 Z = 1.0 / 6.0 - theta2 / 120.0;
231 W = 1.0 / 24.0 - theta2 / 720.0;
234 const double expMinLambda =
exp(-
lambda);
236 if (lambda2 > 1
e-9) {
237 A = (1.0 - expMinLambda) /
lambda;
238 alpha = 1.0 / (1.0 + theta2 / lambda2);
240 mu = (1 -
lambda + (0.5 * lambda2) - expMinLambda) / lambda3;
242 A = 1.0 -
lambda / 2.0 + lambda2 / 6.0;
243 beta = 0.5 -
lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0;
244 mu = 1.0 / 6.0 -
lambda / 24.0 + lambda2 / 120.0 - lambda3 / 720.0;
248 const double C =
alpha * (
mu - upsilon) + upsilon;
250 return A * I_3x3 +
B * Wx +
C * Wx * Wx;
261 throw std::runtime_error(
"Similarity3::Logmap: derivative not implemented");
267 const auto w =
v.head<3>();
268 const auto u =
v.segment<3>(3);
271 throw std::runtime_error(
"Similarity3::Expmap: derivative not implemented");
278 os <<
"[" <<
p.rotation().xyz().transpose() <<
" "
279 <<
p.translation().transpose() <<
" " <<
p.scale() <<
"]\';";
286 T.bottomRows<1>() << 0, 0, 0, 1.0 /
s_;
double scale() const
Return the scale.
static Vector7 Logmap(const Similarity3 &s, OptionalJacobian< 7, 7 > Hm={})
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
bool equals(const Similarity3 &sim, double tol) const
Compare with tolerance.
std::pair< Point3, Point3 > Point3Pair
static Matrix4 wedge(const Vector7 &xi)
Implementation of Similarity3 transform.
std::vector< Point3Pair > Point3Pairs
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
static double calculateScale(const Point3Pairs &d_abPointPairs, const Rot3 &aRb)
Form inner products x and y and calculate scale.
void print(const std::string &s="") const
static Matrix3 calculateH(const Point3Pairs &d_abPointPairs)
Form outer product H.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Jet< T, N > sin(const Jet< T, N > &f)
GaussianFactorGraphValuePair Y
static Rot3 ClosestTo(const Matrix3 &M)
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
Matrix7 AdjointMap() const
Project from one tangent space to another.
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, const Point3Pair ¢roids)
Subtract centroids from point pairs.
Eigen::Triplet< double > T
const EIGEN_DEVICE_FUNC LogReturnType log() const
static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb, const Point3Pair ¢roids)
This method estimates the similarity transform from differences point pairs,.
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
ofstream os("timeSchurFactors.csv")
Similarity3()
Default constructor.
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Similarity3 inverse() const
Return the inverse.
Matrix3 skewSymmetric(double wx, double wy, double wz)
double beta(double a, double b)
mat1 topRightCorner(size/2, size/2)
Jet< T, N > cos(const Jet< T, N > &f)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
bool operator==(const Similarity3 &other) const
Exact equality.
static Similarity3 Align(const Point3Pairs &abPointPairs)
void print(const std::string &s="") const
Print with optional string.
Point3 translation() const
Return a GTSAM translation.
EIGEN_DEVICE_FUNC const Scalar & q
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
static Similarity3 Identity()
Return an identity transform.
Eigen::Block< Derived > topLeftCorner(MatrixBase< Derived > &m, int rows, int cols)
Base class and basic functions for Manifold types.
std::vector< std::pair< Pose3, Pose3 > > Pose3Pairs
Point3 transformFrom(const Point3 &p, OptionalJacobian< 3, 7 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Action on a point p is s*(R*p+t)
Rot3 inverse() const
inverse of a rotation
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, const Rot3 &aRb)
This method estimates the similarity transform from point pairs, given a known or estimated rotation.
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Matrix< Scalar, Dynamic, Dynamic > C
static Matrix3 GetV(Vector3 w, double lambda)
Calculate expmap and logmap coefficients.
Rot3 rotation() const
Return a GTSAM rotation.
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Array< int, Dynamic, 1 > v
Matrix4 matrix() const
Calculate 4*4 matrix group equivalent.
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
bool equals(const Rot3 &p, double tol=1e-9) const
Similarity3 operator*(const Similarity3 &S) const
Composition.
Jet< T, N > sqrt(const Jet< T, N > &f)
Rot2 R(Rot2::fromAngle(0.1))
3D Pose manifold SO(3) x R^3 and group SE(3)
static Similarity3 Expmap(const Vector7 &v, OptionalJacobian< 7, 7 > Hm={})
gtsam
Author(s):
autogenerated on Sun Feb 16 2025 04:03:11