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

Now switched to full ZDim*Dim blocks, no more hacky calibration splitting...

release/4.3a0
dellaert 2015-02-22 22:52:31 +01:00
parent bc0bddf7c6
commit 8619b04cd7
2 changed files with 15 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/geometry/CameraSet.h
View File

@ -27,7 +27,6 @@ namespace gtsam {
/** /**
* @brief A set of cameras, all with their own calibration * @brief A set of cameras, all with their own calibration
* Assumes that a camera is laid out as 6 Pose3 parameters then calibration
*/ */
template<class CAMERA> template<class CAMERA>
class CameraSet: public std::vector<CAMERA> { class CameraSet: public std::vector<CAMERA> {
@ -64,8 +63,8 @@ protected:
public: public:
/// Definitions for blocks of F /// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, 6> MatrixZ6; // F typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
typedef std::vector<MatrixZ6> FBlocks; typedef std::vector<MatrixZD> FBlocks;
/** /**
* print * print
@ -92,15 +91,14 @@ public:
} }
/** /**
* Project a point, with derivatives in this, point, and calibration * Project a point, with derivatives in CameraSet and Point3
* Note that F is a sparse block-diagonal matrix, so instead of a large dense * Note that F is a sparse block-diagonal matrix, so instead of a large dense
* matrix this function returns the diagonal blocks. * matrix this function returns the diagonal blocks.
* throws CheiralityException * throws CheiralityException
*/ */
std::vector<Z> project(const Point3& point, // std::vector<Z> project2(const Point3& point, //
boost::optional<FBlocks&> F = boost::none, // boost::optional<FBlocks&> F = boost::none, //
boost::optional<Matrix&> E = boost::none, // boost::optional<Matrix&> E = boost::none) const {
boost::optional<Matrix&> H = boost::none) const {
// Allocate result // Allocate result
size_t m = this->size(); size_t m = this->size();
@ -109,22 +107,16 @@ public:
// Allocate derivatives // Allocate derivatives
if (E) if (E)
E->resize(ZDim * m, 3); E->resize(ZDim * m, 3);
if (H && Dim > 6)
H->resize(ZDim * m, Dim - 6);
Eigen::Matrix<double, ZDim, 6> Fi;
Eigen::Matrix<double, ZDim, 3> Ei;
Eigen::Matrix<double, ZDim, Dim - 6> Hi;
// Project and fill derivatives // Project and fill derivatives
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); Eigen::Matrix<double, ZDim, Dim> Fi;
Eigen::Matrix<double, ZDim, 3> Ei;
z[i] = this->at(i).project2(point, F ? &Fi : 0, E ? &Ei : 0);
if (F) if (F)
F->push_back(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)
H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
} }
return z; return z;
@ -150,7 +142,7 @@ public:
/// Calculate vector of re-projection errors /// Calculate vector of re-projection errors
Vector reprojectionErrors(const Point3& point, Vector reprojectionErrors(const Point3& point,
const std::vector<Z>& measured) const { const std::vector<Z>& measured) const {
return ErrorVector(project(point), measured); return ErrorVector(project2(point), measured);
} }
/// Calculate vector of re-projection errors, from point at infinity /// Calculate vector of re-projection errors, from point at infinity

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

@ -44,31 +44,28 @@ TEST(CameraSet, Pinhole) {
// Check measurements // Check measurements
Point2 expected; Point2 expected;
ZZ z = set.project(p); ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0])); EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1])); EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole // Calculate expected derivatives using Pinhole
Matrix43 actualE; Matrix43 actualE;
Matrix43 actualH;
Matrix F1; Matrix F1;
{ {
Matrix23 E1; Matrix23 E1;
Matrix23 H1; Matrix23 H1;
camera.project(p, F1, E1, H1); camera.project2(p, F1, E1);
actualE << E1, E1; actualE << E1, E1;
actualH << H1, H1;
} }
// Check computed derivatives // Check computed derivatives
CameraSet<Camera>::FBlocks F; CameraSet<Camera>::FBlocks F;
Matrix E, H; Matrix E, H;
set.project(p, F, E, H); set.project2(p, F, E);
LONGS_EQUAL(2,F.size()); LONGS_EQUAL(2,F.size());
EXPECT(assert_equal(F1, F[0])); EXPECT(assert_equal(F1, F[0]));
EXPECT(assert_equal(F1, F[1])); EXPECT(assert_equal(F1, F[1]));
EXPECT(assert_equal(actualE, E)); EXPECT(assert_equal(actualE, E));
EXPECT(assert_equal(actualH, H));
// Check errors // Check errors
ZZ measured; ZZ measured;
@ -102,7 +99,7 @@ TEST(CameraSet, Stereo) {
// Check measurements // Check measurements
StereoPoint2 expected(0, -1, 0); StereoPoint2 expected(0, -1, 0);
ZZ z = set.project(p); ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0])); EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1])); EXPECT(assert_equal(expected, z[1]));
@ -111,14 +108,14 @@ TEST(CameraSet, Stereo) {
Matrix F1; Matrix F1;
{ {
Matrix33 E1; Matrix33 E1;
camera.project(p, F1, E1); camera.project2(p, F1, E1);
actualE << E1, E1; actualE << E1, E1;
} }
// Check computed derivatives // Check computed derivatives
CameraSet<StereoCamera>::FBlocks F; CameraSet<StereoCamera>::FBlocks F;
Matrix E; Matrix E;
set.project(p, F, E); set.project2(p, F, E);
LONGS_EQUAL(2,F.size()); LONGS_EQUAL(2,F.size());
EXPECT(assert_equal(F1, F[0])); EXPECT(assert_equal(F1, F[0]));
EXPECT(assert_equal(F1, F[1])); EXPECT(assert_equal(F1, F[1]));