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

reprojectionErrors

release/4.3a0
dellaert 2015-02-22 21:48:21 +01:00
parent c9536523bf
commit 81538aac55
2 changed files with 104 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
gtsam/geometry/CameraSet.h
View File

@ -43,6 +43,24 @@ protected:
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
/// Make a vector of re-projection errors
static Vector ErrorVector(const std::vector<Z>& predicted,
const std::vector<Z>& measured) {
// Check size
size_t m = predicted.size();
if (measured.size() != m)
throw std::runtime_error("CameraSet::errors: size mismatch");
// Project and fill derivatives
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
Z e = predicted[i] - measured[i];
b.segment<ZDim>(row) = e.vector();
}
return b;
}
public:
/// Definitions for blocks of F
@ -77,28 +95,71 @@ public:
* Project a point, with derivatives in this, point, and calibration
* throws CheiralityException
*/
std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F =
boost::none, boost::optional<Matrix&> E = boost::none,
std::vector<Z> project(const Point3& point, //
boost::optional<Matrix&> F = boost::none, //
boost::optional<Matrix&> E = boost::none, //
boost::optional<Matrix&> H = boost::none) const {
size_t nrCameras = this->size();
if (F) F->resize(ZDim * nrCameras, 6);
if (E) E->resize(ZDim * nrCameras, 3);
if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6);
std::vector<Z> z(nrCameras);
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
for (size_t i = 0; i < nrCameras; i++) {
Eigen::Matrix<double, ZDim, 6> Fi;
Eigen::Matrix<double, ZDim, 3> Ei;
Eigen::Matrix<double, ZDim, Dim - 6> Hi;
// Allocate derivatives
if (F)
F->resize(ZDim * m, 6);
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);
if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
if (F)
F->block<ZDim, 6>(ZDim * i, 0) = Fi;
if (E)
E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H)
H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
}
return z;
}
/**
* Project a point, with derivatives in this, point, and calibration
* throws CheiralityException
*/
std::vector<Z> projectAtInfinity(const Point3& point) const {
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
// Project and fill derivatives
for (size_t i = 0; i < m; i++)
z[i] = this->at(i).projectPointAtInfinity(point);
return z;
}
/// Calculate vector of re-projection errors
Vector reprojectionErrors(const Point3& point,
const std::vector<Z>& measured) const {
return ErrorVector(project(point), measured);
}
/// Calculate vector of re-projection errors, from point at infinity
// TODO: take Unit3 instead
Vector reprojectionErrorsAtInfinity(const Point3& point,
const std::vector<Z>& measured) const {
return ErrorVector(projectAtInfinity(point), measured);
}
private:
/// Serialization function

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

@ -37,16 +37,16 @@ TEST(CameraSet, Pinhole) {
set.push_back(camera);
set.push_back(camera);
Point3 p(0, 0, 1);
CHECK(assert_equal(set, set));
EXPECT(assert_equal(set, set));
CameraSet<Camera> set2 = set;
set2.push_back(camera);
CHECK(!set.equals(set2));
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
ZZ z = set.project(p);
CHECK(assert_equal(expected, z[0]));
CHECK(assert_equal(expected, z[1]));
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix46 actualF;
@ -65,9 +65,27 @@ TEST(CameraSet, Pinhole) {
// Check computed derivatives
Matrix F, E, H;
set.project(p, F, E, H);
CHECK(assert_equal(actualF, F));
CHECK(assert_equal(actualE, E));
CHECK(assert_equal(actualH, H));
EXPECT(assert_equal(actualF, F));
EXPECT(assert_equal(actualE, E));
EXPECT(assert_equal(actualH, H));
// Check errors
ZZ measured;
measured.push_back(Point2(1, 2));
measured.push_back(Point2(3, 4));
Vector4 expectedV;
// reprojectionErrors
expectedV << -1, -2, -3, -4;
Vector actualV = set.reprojectionErrors(p, measured);
EXPECT(assert_equal(expectedV, actualV));
// reprojectionErrorsAtInfinity
EXPECT(
assert_equal(Point3(0, 0, 1),
camera.backprojectPointAtInfinity(Point2())));
actualV = set.reprojectionErrorsAtInfinity(p, measured);
EXPECT(assert_equal(expectedV, actualV));
}
/* ************************************************************************* */
@ -84,8 +102,8 @@ TEST(CameraSet, Stereo) {
// Check measurements
StereoPoint2 expected(0, -1, 0);
ZZ z = set.project(p);
CHECK(assert_equal(expected, z[0]));
CHECK(assert_equal(expected, z[1]));
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix66 actualF;
@ -101,8 +119,8 @@ TEST(CameraSet, Stereo) {
// Check computed derivatives
Matrix F, E;
set.project(p, F, E);
CHECK(assert_equal(actualF, F));
CHECK(assert_equal(actualE, E));
EXPECT(assert_equal(actualF, F));
EXPECT(assert_equal(actualE, E));
}
/* ************************************************************************* */