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:
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
typedef std::pair<Key, MatrixZD> FBlock; // Fblocks
typedef Eigen::Matrix<double, ZDim, 6> MatrixZ6; // F
typedef std::vector<MatrixZ6> FBlocks;
/**
* print
@ -93,10 +93,12 @@ public:
/**
* 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
*/
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&> H = boost::none) const {
@ -105,8 +107,6 @@ public:
std::vector<Z> z(m);
// Allocate derivatives
if (F)
F->resize(ZDim * m, 6);
if (E)
E->resize(ZDim * m, 3);
if (H && Dim > 6)
@ -120,7 +120,7 @@ public:
for (size_t i = 0; i < m; i++) {
z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
if (F)
F->block<ZDim, 6>(ZDim * i, 0) = Fi;
F->push_back(Fi);
if (E)
E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H)

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

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