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

Templated instead of two identical functions

release/4.3a0
dellaert 2015-03-05 22:47:28 -08:00
parent 7366549c7f
commit 6c72c29a56
1 changed files with 10 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
gtsam/geometry/CameraSet.h
View File

@ -93,85 +93,50 @@ public:
}
/**
* Project a point, with derivatives in CameraSet and Point3
* Project a point (posibly Unit3 at infinity), with derivatives
* 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> project2(const Point3& point, //
template<class POINT>
std::vector<Z> project2(const POINT& point, //
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
static const int N = FixedDimension<POINT>::value;
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
// Allocate derivatives
if (E)
E->resize(ZDim * m, 3);
E->resize(ZDim * m, N);
if (Fs)
Fs->resize(m);
// Project and fill derivatives
for (size_t i = 0; i < m; i++) {
MatrixZD Fi;
Eigen::Matrix<double, ZDim, 3> Ei;
Eigen::Matrix<double, ZDim, N> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
if (Fs)
(*Fs)[i] = Fi;
if (E)
E->block<ZDim, 3>(ZDim * i, 0) = Ei;
}
return z;
}
/**
* Project a point at infinity, with derivatives in this, point, and calibration
* throws CheiralityException
*/
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++) {
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;
E->block<ZDim, N>(ZDim * i, 0) = Ei;
}
return z;
}
/// Calculate vector [project2(point)-z] of re-projection errors
Vector reprojectionError(const Point3& point, const std::vector<Z>& measured,
template<class POINT>
Vector reprojectionError(const POINT& 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);
}
/// Calculate vector [project2(point)-z] of re-projection errors, from point at infinity
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);
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F