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
* Assumes that a camera is laid out as 6 Pose3 parameters then calibration
*/
template<class CAMERA>
class CameraSet: public std::vector<CAMERA> {
@ -64,8 +63,8 @@ protected:
public:
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, 6> MatrixZ6; // F
typedef std::vector<MatrixZ6> FBlocks;
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
typedef std::vector<MatrixZD> FBlocks;
/**
* 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
* matrix this function returns the diagonal blocks.
* throws CheiralityException
*/
std::vector<Z> project(const Point3& point, //
std::vector<Z> project2(const Point3& point, //
boost::optional<FBlocks&> F = boost::none, //
boost::optional<Matrix&> E = boost::none, //
boost::optional<Matrix&> H = boost::none) const {
boost::optional<Matrix&> E = boost::none) const {
// Allocate result
size_t m = this->size();
@ -109,22 +107,16 @@ public:
// Allocate derivatives
if (E)
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
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)
F->push_back(Fi);
if (E)
E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H)
H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
}
return z;
@ -150,7 +142,7 @@ public:
/// Calculate vector of re-projection errors
Vector reprojectionErrors(const Point3& point,
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

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

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