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

transform_to, but with bug in Jacobains...

release/4.3a0
Frank Dellaert 2013-12-19 04:07:20 +00:00
parent e5d7e3ce8a
commit 197a3efef9
2 changed files with 43 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/geometry/EssentialMatrix.h
View File

@ -7,7 +7,7 @@
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Sphere2.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
@ -110,6 +110,32 @@ public:
return E_;
}
/**
* @brief takes point in world coordinates and transforms it to pose with |t|==1
* @param p point in world coordinates
* @param DE optional 3*5 Jacobian wrpt to E
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in pose coordinates
*/
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const {
Pose3 pose(aRb_, aTb_.point3());
Point3 q = pose.transform_to(p, DE, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
Matrix H(3, 5);
std::cout << *DE << std::endl << std::endl;
std::cout << aTb_.basis() << std::endl << std::endl;
H << DE->block < 3, 3 > (0, 0), DE->block < 3, 3 > (0, 3) * aTb_.basis();
std::cout << H << std::endl << std::endl;
*DE = H;
}
return q;
}
/// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H = boost::none) const {

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

@ -7,8 +7,8 @@
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -44,6 +44,20 @@ TEST (EssentialMatrix, retract2) {
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); }
TEST (EssentialMatrix, transform_to) {
EssentialMatrix E(aRb, aTb);
//EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
static Point3 P(0.2,0.7,-2);
Matrix actH1, actH2;
E.transform_to(P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_to_, E,P),
expH2 = numericalDerivative22(transform_to_, E,P);
EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;