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

Now return FBlocks as derivatives

release/4.3a0
dellaert 2015-02-22 22:32:25 +01:00
parent 66d12a1c30
commit 1e62f31064
2 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/CameraSet.h
View File

@ -64,8 +64,8 @@ protected:
public: public:
/// Definitions for blocks of F /// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F typedef Eigen::Matrix<double, ZDim, 6> MatrixZ6; // F
typedef std::pair<Key, MatrixZD> FBlock; // Fblocks typedef std::vector<MatrixZ6> FBlocks;
/** /**
* print * print
@ -93,10 +93,12 @@ public:
/** /**
* Project a point, with derivatives in this, point, and calibration * Project a point, with derivatives in this, point, and calibration
* Note that F is a sparse block-diagonal matrix, so instead of a large dense
* matrix this function returns the diagonal blocks.
* throws CheiralityException * throws CheiralityException
*/ */
std::vector<Z> project(const Point3& point, // std::vector<Z> project(const Point3& point, //
boost::optional<Matrix&> F = boost::none, // boost::optional<FBlocks&> F = boost::none, //
boost::optional<Matrix&> E = boost::none, // boost::optional<Matrix&> E = boost::none, //
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
@ -105,8 +107,6 @@ public:
std::vector<Z> z(m); std::vector<Z> z(m);
// Allocate derivatives // Allocate derivatives
if (F)
F->resize(ZDim * m, 6);
if (E) if (E)
E->resize(ZDim * m, 3); E->resize(ZDim * m, 3);
if (H && Dim > 6) if (H && Dim > 6)
@ -120,7 +120,7 @@ public:
for (size_t i = 0; i < m; i++) { for (size_t i = 0; i < m; i++) {
z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
if (F) if (F)
F->block<ZDim, 6>(ZDim * i, 0) = Fi; F->push_back(Fi);
if (E) if (E)
E->block<ZDim, 3>(ZDim * i, 0) = Ei; E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H) if (H)

22
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -49,23 +49,24 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal(expected, z[1])); EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole // Calculate expected derivatives using Pinhole
Matrix46 actualF;
Matrix43 actualE; Matrix43 actualE;
Matrix43 actualH; Matrix43 actualH;
Matrix F1;
{ {
Matrix26 F1;
Matrix23 E1; Matrix23 E1;
Matrix23 H1; Matrix23 H1;
camera.project(p, F1, E1, H1); camera.project(p, F1, E1, H1);
actualE << E1, E1; actualE << E1, E1;
actualF << F1, F1;
actualH << H1, H1; actualH << H1, H1;
} }
// Check computed derivatives // Check computed derivatives
Matrix F, E, H; CameraSet<Camera>::FBlocks F;
Matrix E, H;
set.project(p, F, E, H); set.project(p, F, E, H);
EXPECT(assert_equal(actualF, F)); LONGS_EQUAL(2,F.size());
EXPECT(assert_equal(F1, F[0]));
EXPECT(assert_equal(F1, F[1]));
EXPECT(assert_equal(actualE, E)); EXPECT(assert_equal(actualE, E));
EXPECT(assert_equal(actualH, H)); EXPECT(assert_equal(actualH, H));
@ -106,20 +107,21 @@ TEST(CameraSet, Stereo) {
EXPECT(assert_equal(expected, z[1])); EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole // Calculate expected derivatives using Pinhole
Matrix66 actualF;
Matrix63 actualE; Matrix63 actualE;
Matrix F1;
{ {
Matrix36 F1;
Matrix33 E1; Matrix33 E1;
camera.project(p, F1, E1); camera.project(p, F1, E1);
actualE << E1, E1; actualE << E1, E1;
actualF << F1, F1;
} }
// Check computed derivatives // Check computed derivatives
Matrix F, E; CameraSet<StereoCamera>::FBlocks F;
Matrix E;
set.project(p, F, E); set.project(p, F, E);
EXPECT(assert_equal(actualF, F)); LONGS_EQUAL(2,F.size());
EXPECT(assert_equal(F1, F[0]));
EXPECT(assert_equal(F1, F[1]));
EXPECT(assert_equal(actualE, E)); EXPECT(assert_equal(actualE, E));
} }