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
77  Rot3 c1Rc2 = rotation().conjugate(cRb);
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 
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
Vector vB(size_t i)
ostream & operator<<(ostream &os, const EssentialMatrix &E)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Eigen::Vector3d Vector3
Definition: Vector.h:43
const Unit3 & direction() const
Direction.
Rot2 R(Rot2::fromAngle(0.1))
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
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
gtsam::Rot3 cRb
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
const double dt
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Rot3 conjugate(const Rot3 &cRb) const
Definition: Rot3.h:321
Vector3 xyz(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:160
P rotate(const T &r, const P &pt)
Definition: lieProxies.h:47
Vector vA(size_t i)
const Rot3 & rotation() const
Rotation.
RealScalar s
static const Point3 point3(0.08, 0.08, 0.0)
EIGEN_DEVICE_FUNC const Scalar & q
traits
Definition: chartTesting.h:28
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
DiscreteKey E(5, 2)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
ofstream os("timeSchurFactors.csv")
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:371
Matrix3 matrix() const
Definition: Rot3M.cpp:218
#define HR
Definition: mincover.c:26
float * p
static double error
Definition: testRot3.cpp:37
void print(const std::string &s="") const
Definition: Rot3.cpp:33
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Point3 point3(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
Vector3 Point3
Definition: Point3.h:38
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Matrix3 transpose() const
Definition: Rot3M.cpp:143
istream & operator>>(istream &is, EssentialMatrix &E)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:12