12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved to header as friend function

release/4.3a0
Frank Dellaert 2014-01-03 17:49:45 -05:00
parent d6864b0478
commit 5002f3da8a
2 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/EssentialMatrix.h
View File

@ -145,6 +145,17 @@ public:
return q; return q;
} }
/**
* Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C
*/
friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
Rot3 c1Rc2 = E.aRb_.conjugate(cRb);
Sphere2 c1Tc2 = cRb * E.aTb_;
return EssentialMatrix(c1Rc2, c1Tc2);
}
/// epipolar error, algebraic /// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, // double error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {

13
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -66,19 +66,6 @@ TEST (EssentialMatrix, transform_to) {
//************************************************************************* //*************************************************************************
/**
* Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C
*/
EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
Rot3 b1Rb2 = E.rotation();
Sphere2 b1Tb2 = E.direction();
Rot3 c1Rc2 = b1Rb2.conjugate(cRb);
Sphere2 c1Tc2 = cRb * b1Tb2;
return EssentialMatrix(c1Rc2, c1Tc2);
}
TEST (EssentialMatrix, rotate) { TEST (EssentialMatrix, rotate) {
// Suppose the essential matrix is specified in a body coordinate frame B // Suppose the essential matrix is specified in a body coordinate frame B
// which is rotated with respect to the camera frame C, via rotation bRc. // which is rotated with respect to the camera frame C, via rotation bRc.