Go to the documentation of this file.
34 cout << (
s.empty() ?
"R: " :
s +
" ");
41 uniform_real_distribution<double> randomAngle(-
M_PI,
M_PI);
42 double angle = randomAngle(
rng);
43 return AxisAngle(axis, angle);
58 throw std::runtime_error(
"AlignSinglePair: axis has Nans");
61 const Matrix3
P = I_3x3 -
z *
z.transpose();
66 const double u =
x.dot(b_po);
67 const double v =
y.dot(b_po);
69 return Rot3::AxisAngle(
z, -angle);
81 Rot3 i_R_b = AlignPair(a_p.
cross(b_p), a_p, b_p);
85 Unit3 i_q = i_R_b * b_q;
89 Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
94 Rot3 a_R_b = a_R_i * i_R_b;
99 bool Rot3::equals(
const Rot3 &
R,
double tol)
const {
113 if (Hp) *Hp =
q.basis().transpose() *
matrix() * Dp;
114 if (
HR) *
HR = -
q.basis().transpose() *
matrix() *
p.skew();
123 if (Hp) *Hp =
q.basis().transpose() *
matrix().transpose () * Dp;
124 if (
HR) *
HR =
q.basis().transpose() *
q.skew();
137 const Matrix3& Rt = transpose();
139 const double wx =
q.x(), wy =
q.y(), wz =
q.z();
141 *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
156 throw invalid_argument(
"Argument to Rot3::column must be 1, 2, or 3");
165 #ifdef GTSAM_USE_QUATERNIONS
172 std::tie(
I,
q) =
RQ(
m, qHm);
185 if (
H)
H->row(0).swap(
H->row(2));
230 pair<Unit3, double> Rot3::axisAngle()
const {
237 return SO3::ExpmapDerivative(
x);
242 return SO3::LogmapDerivative(
x);
247 const double x = -
atan2(-
A(2, 1),
A(2, 2));
248 const auto Qx = Rot3::Rx(-
x).matrix();
249 const Matrix3
B =
A * Qx;
251 const double y = -
atan2(
B(2, 0),
B(2, 2));
252 const auto Qy = Rot3::Ry(-
y).matrix();
253 const Matrix3
C =
B * Qy;
255 const double z = -
atan2(-
C(1, 0),
C(1, 1));
256 const auto Qz = Rot3::Rz(-
z).matrix();
257 const Matrix3
R =
C * Qz;
261 throw std::runtime_error(
262 "Rot3::RQ : Derivative undefined at singularity (gimbal lock)");
264 auto atan_d1 = [](
double y,
double x) {
return x / (
x *
x +
y *
y); };
265 auto atan_d2 = [](
double y,
double x) {
return -
y / (
x *
x +
y *
y); };
267 const auto sx = -Qx(2, 1),
cx = Qx(1, 1);
268 const auto sy = -Qy(0, 2),
cy = Qy(0, 0);
270 *
H = Matrix39::Zero();
272 (*H)(0, 5) = atan_d1(
A(2, 1),
A(2, 2));
273 (*H)(0, 8) = atan_d2(
A(2, 1),
A(2, 2));
277 (*H)(1, 2) = -atan_d1(
B(2, 0),
B(2, 2));
278 const auto yHb22 = -atan_d2(
B(2, 0),
B(2, 2));
279 (*H)(1, 5) = yHb22 * sx;
280 (*H)(1, 8) = yHb22 *
cx;
285 const auto c10Hx = (
A(1, 1) *
cx -
A(1, 2) * sx) * sy;
286 const auto c10Hy =
A(1, 2) *
cx *
cy +
A(1, 1) *
cy * sx -
A(1, 0) * sy;
287 Vector9 c10HA = c10Hx *
H->row(0) + c10Hy *
H->row(1);
292 const auto c11Hx = -
A(1, 2) *
cx -
A(1, 1) * sx;
293 Vector9 c11HA = c11Hx *
H->row(0);
297 H->block<1, 9>(2, 0) =
298 atan_d1(
C(1, 0),
C(1, 1)) * c10HA + atan_d2(
C(1, 0),
C(1, 1)) * c11HA;
302 return make_pair(
R, xyz);
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
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
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
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
const Eigen::IOFormat & matlabFormat()
ofstream os("timeSchurFactors.csv")
ostream & operator<<(ostream &os, const Rot3 &R)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
3D rotation represented as a rotation matrix or quaternion
3*3 matrix representation of SO(3)
double dot(const Unit3 &q, OptionalJacobian< 1, 2 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Return dot product with q.
void print(const Matrix &A, const string &s, ostream &stream)
EIGEN_DEVICE_FUNC const Scalar & q
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={})
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Matrix< Scalar, Dynamic, Dynamic > C
Array< int, Dynamic, 1 > v
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
P unrotate(const T &r, const P &pt)
Represents a 3D point on a unit sphere.
Unit3 cross(const Unit3 &q) const
Cross-product between two Unit3s.
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
P rotate(const T &r, const P &pt)
Rot2 R(Rot2::fromAngle(0.1))
pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H)
Vector3_ operator*(const Double_ &s, const Vector3_ &v)
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:01