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

project2 on Unit3, with derivatives

release/4.3a0
dellaert 2015-03-05 22:44:05 -08:00
parent 4594c2dee5
commit 7366549c7f
3 changed files with 79 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
gtsam/geometry/CameraSet.h
View File

@ -57,7 +57,7 @@ protected:
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();
b.segment < ZDim > (row) = e.vector();
}
return b;
}
@ -109,6 +109,8 @@ public:
// Allocate derivatives
if (E)
E->resize(ZDim * m, 3);
if (Fs)
Fs->resize(m);
// Project and fill derivatives
for (size_t i = 0; i < m; i++) {
@ -116,7 +118,7 @@ public:
Eigen::Matrix<double, ZDim, 3> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
if (Fs)
Fs->push_back(Fi);
(*Fs)[i] = Fi;
if (E)
E->block<ZDim, 3>(ZDim * i, 0) = Ei;
}
@ -125,18 +127,33 @@ public:
}
/**
* Project a point, with derivatives in this, point, and calibration
* Project a point at infinity, with derivatives in this, point, and calibration
* throws CheiralityException
*/
std::vector<Z> projectAtInfinity(const Point3& point) const {
std::vector<Z> project2(const Unit3& point, //
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
// Allocate derivatives
if (E)
E->resize(ZDim * m, 2);
if (Fs)
Fs->resize(m);
// Project and fill derivatives
for (size_t i = 0; i < m; i++)
z[i] = this->at(i).projectPointAtInfinity(point);
for (size_t i = 0; i < m; i++) {
MatrixZD Fi;
Eigen::Matrix<double, ZDim, 2> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
if (Fs)
(*Fs)[i] = Fi;
if (E)
E->block<ZDim, 2>(ZDim * i, 0) = Ei;
}
return z;
}
@ -149,10 +166,10 @@ public:
}
/// Calculate vector [project2(point)-z] of re-projection errors, from point at infinity
// TODO: take Unit3 instead
Vector reprojectionErrorAtInfinity(const Point3& point,
const std::vector<Z>& measured) const {
return ErrorVector(projectAtInfinity(point), measured);
Vector reprojectionError(const Unit3& point, const std::vector<Z>& measured,
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
return ErrorVector(project2(point,Fs,E), measured);
}
/**
@ -180,7 +197,7 @@ public:
const Matrix23 Ei_P = E.block<ZDim, 3>(ZDim * i, 0) * P;
// D = (Dx2) * ZDim
augmentedHessian(i, m) = Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
augmentedHessian(i, m) = Fi.transpose() * b.segment < ZDim > (ZDim * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
@ -212,7 +229,7 @@ public:
assert(keys.size()==Fs.size());
assert(keys.size()<=allKeys.size());
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
@ -245,7 +262,7 @@ public:
// vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal()
+ Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
+ Fi.transpose() * b.segment < ZDim > (ZDim * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )

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

@ -50,12 +50,12 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix43 actualE;
Matrix actualE;
Matrix29 F1;
{
Matrix23 E1;
Matrix23 H1;
camera.project2(p, F1, E1);
actualE.resize(4,3);
actualE << E1, E1;
}
@ -114,11 +114,22 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix()));
// reprojectionErrorAtInfinity
Unit3 pointAtInfinity(0, 0, 1000);
EXPECT(
assert_equal(Point3(0, 0, 1),
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
actualV = set.reprojectionErrorAtInfinity(p, measured);
actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
EXPECT(assert_equal(expectedV, actualV));
LONGS_EQUAL(2, Fs.size());
{
Matrix22 E1;
camera.project2(pointAtInfinity, F1, E1);
actualE.resize(4,2);
actualE << E1, E1;
}
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
EXPECT(assert_equal(actualE, E));
}
/* ************************************************************************* */

51
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -53,12 +53,12 @@ TEST(PinholeSet, Stereo) {
}
// Check computed derivatives
PinholeSet<CalibratedCamera>::FBlocks F;
PinholeSet<CalibratedCamera>::FBlocks Fs;
Matrix E;
set.project2(p, F, E);
LONGS_EQUAL(2, F.size());
EXPECT(assert_equal(F1, F[0]));
EXPECT(assert_equal(F1, F[1]));
set.project2(p, Fs, E);
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
EXPECT(assert_equal(actualE, E));
// Instantiate triangulateSafe
@ -90,23 +90,25 @@ TEST(PinholeSet, Pinhole) {
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix43 actualE;
Matrix actualE;
Matrix F1;
{
Matrix23 E1;
Matrix23 H1;
camera.project2(p, F1, E1);
actualE.resize(4, 3);
actualE << E1, E1;
}
// Check computed derivatives
PinholeSet<Camera>::FBlocks F;
Matrix 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));
{
PinholeSet<Camera>::FBlocks Fs;
Matrix E;
set.project2(p, Fs, E);
EXPECT(assert_equal(actualE, E));
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
}
// Check errors
ZZ measured;
@ -120,15 +122,30 @@ TEST(PinholeSet, Pinhole) {
EXPECT(assert_equal(expectedV, actualV));
// reprojectionErrorAtInfinity
Unit3 pointAtInfinity(0, 0, 1000);
{
Matrix22 E1;
camera.project2(pointAtInfinity, F1, E1);
actualE.resize(4, 2);
actualE << E1, E1;
}
EXPECT(
assert_equal(Point3(0, 0, 1),
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
actualV = set.reprojectionErrorAtInfinity(p, measured);
{
PinholeSet<Camera>::FBlocks Fs;
Matrix E;
actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
EXPECT(assert_equal(actualE, E));
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
}
EXPECT(assert_equal(expectedV, actualV));
// Instantiate triangulateSafe
TriangulationParameters params;
TriangulationResult actual = set.triangulateSafe(z,params);
TriangulationResult actual = set.triangulateSafe(z, params);
CHECK(actual.degenerate());
}