EssentialMatrix.cpp
Go to the documentation of this file.
1 /*
2  * @file EssentialMatrix.cpp
3  * @brief EssentialMatrix class
4  * @author Frank Dellaert
5  * @date December 5, 2014
6  */
7 
9 #include <iostream>
10 
11 using namespace std;
12 
13 namespace gtsam {
14 
15 /* ************************************************************************* */
16 EssentialMatrix EssentialMatrix::FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
19  if (H1)
20  *H1 << I_3x3, Matrix23::Zero();
21  if (H2)
22  *H2 << Matrix32::Zero(), I_2x2;
23  return EssentialMatrix(aRb, aTb);
24 }
25 
26 /* ************************************************************************* */
27 EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
29  const Rot3& aRb = aPb.rotation();
30  const Point3& aTb = aPb.translation();
31  if (!H) {
32  // just make a direction out of translation and create E
33  Unit3 direction(aTb);
34  return EssentialMatrix(aRb, direction);
35  } else {
36  // Calculate the 5*6 Jacobian H = D_E_1P2
37  // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
38  // First get 2*3 derivative from Unit3::FromPoint3
39  Matrix23 D_direction_1T2;
40  Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2);
41  *H << I_3x3, Z_3x3, //
42  Matrix23::Zero(), D_direction_1T2 * aRb.matrix();
43  return EssentialMatrix(aRb, direction);
44  }
45 }
46 
47 /* ************************************************************************* */
48 void EssentialMatrix::print(const string& s) const {
49  cout << s;
50  rotation().print("R:\n");
51  direction().print("d: ");
52 }
53 
54 /* ************************************************************************* */
56  OptionalJacobian<3, 3> Dpoint) const {
57  Pose3 pose(rotation(), direction().point3());
58  Matrix36 DE_;
59  Point3 q = pose.transformTo(p, DE ? &DE_ : 0, Dpoint);
60  if (DE) {
61  // DE returned by pose.transformTo is 3*6, but we need it to be 3*5
62  // The last 3 columns are derivative with respect to change in translation
63  // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
64  // Duy made an educated guess that this needs to be rotated to the local frame
65  Matrix35 H;
66  H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis();
67  *DE = H;
68  }
69  return q;
70 }
71 
72 /* ************************************************************************* */
75 
76  // The rotation must be conjugated to act in the camera frame
78 
79  if (!HE && !HR) {
80  // Rotate translation direction and return
81  Unit3 c1Tc2 = cRb * direction();
82  return EssentialMatrix(c1Rc2, c1Tc2);
83  } else {
84  // Calculate derivatives
85  Matrix23 D_c1Tc2_cRb; // 2*3
86  Matrix2 D_c1Tc2_aTb; // 2*2
87  Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb);
88  if (HE)
89  *HE << cRb.matrix(), Matrix32::Zero(), //
90  Matrix23::Zero(), D_c1Tc2_aTb;
91  if (HR) {
92  throw runtime_error(
93  "EssentialMatrix::rotate: derivative HR not implemented yet");
94  /*
95  HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
96  HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
97  */
98  }
99  return EssentialMatrix(c1Rc2, c1Tc2);
100  }
101 }
102 
103 /* ************************************************************************* */
104 double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
105  OptionalJacobian<1, 5> H) const {
106  if (H) {
107  // See math.lyx
108  Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
109  Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
110  * direction().basis();
111  *H << HR, HD;
112  }
113  return dot(vA, E_ * vB);
114 }
115 
116 /* ************************************************************************* */
117 ostream& operator <<(ostream& os, const EssentialMatrix& E) {
118  Rot3 R = E.rotation();
119  Unit3 d = E.direction();
120  os.precision(10);
121  os << R.xyz().transpose() << " " << d.point3().transpose() << " ";
122  return os;
123 }
124 
125 /* ************************************************************************* */
126 istream& operator >>(istream& is, EssentialMatrix& E) {
127  double rx, ry, rz, dx, dy, dz;
128  is >> rx >> ry >> rz; // Read the rotation rxyz
129  is >> dx >> dy >> dz; // Read the translation dxyz
130 
131  // Create EssentialMatrix from rotation and translation
132  Rot3 rot = Rot3::RzRyRx(rx, ry, rz);
133  Unit3 dt = Unit3(dx, dy, dz);
134  E = EssentialMatrix(rot, dt);
135 
136  return is;
137 }
138 
139 /* ************************************************************************* */
140 
141 } // gtsam
142 
gtsam::internal::rotation
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:80
H
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
Definition: gnuplot_common_settings.hh:74
EssentialMatrix.h
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::Rot3::print
void print(const std::string &s="") const
Definition: Rot3.cpp:33
aRb
Rot3 aRb
Definition: testFundamentalMatrix.cpp:146
s
RealScalar s
Definition: level1_cplx_impl.h:126
d
static const double d[K][N]
Definition: igam.h:11
dt
const double dt
Definition: testVelocityConstraint.cpp:15
point3
static const Point3 point3(0.08, 0.08, 0.0)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
rot
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
Definition: level1_real_impl.h:79
example1::vB
Vector vB(size_t i)
Definition: testEssentialMatrixFactor.cpp:52
example1::c1Tc2
Point3 c1Tc2
Definition: testEssentialMatrixFactor.cpp:40
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:49
example1::vA
Vector vA(size_t i)
Definition: testEssentialMatrixFactor.cpp:51
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
gtsam::EssentialMatrix
Definition: EssentialMatrix.h:26
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Rot3::conjugate
Rot3 conjugate(const Rot3 &cRb) const
Definition: Rot3.h:321
gtsam::operator<<
ostream & operator<<(ostream &os, const EssentialMatrix &E)
Definition: EssentialMatrix.cpp:117
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
example2::aTb
Point3 aTb
Definition: testEssentialMatrixFactor.cpp:541
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
E
DiscreteKey E(5, 2)
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
example1::c1Rc2
Rot3 c1Rc2
Definition: testEssentialMatrixFactor.cpp:39
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
cRb
gtsam::Rot3 cRb
Definition: testEssentialMatrixFactor.cpp:33
gtsam::operator>>
istream & operator>>(istream &is, EssentialMatrix &E)
Definition: EssentialMatrix.cpp:126
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
Definition: lieProxies.h:47
R
Rot2 R(Rot2::fromAngle(0.1))
HR
#define HR
Definition: mincover.c:26


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:29