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

Isolated calculation of Point Covariance in static method(s) PointCov

release/4.3a0
dellaert 2015-02-20 02:06:47 +01:00
parent 4badb4a10f
commit 843bc7cb82
3 changed files with 178 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

247
gtsam/slam/SmartFactorBase.h
View File

@ -245,12 +245,12 @@ public:
} }
/** /**
* Compute the matrices E and PointCov = inv(E'*E), where E stacks the ZDim*3 derivatives * Compute whitenedError, returning only the derivative E, i.e.,
* of project with respect to the point, given each of the m cameras. * the stacked ZDim*3 derivatives of project with respect to the point.
* Assumes non-degenerate ! * Assumes non-degenerate ! TODO explain this
*/ */
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, Vector whitenedError(const Cameras& cameras, const Point3& point,
const Point3& point) const { Matrix& E) const {
// Project into CameraSet, calculating derivatives // Project into CameraSet, calculating derivatives
std::vector<Z> predicted; std::vector<Z> predicted;
@ -258,12 +258,20 @@ public:
using boost::none; using boost::none;
predicted = cameras.project(point, none, E, none); predicted = cameras.project(point, none, E, none);
} catch (CheiralityException&) { } catch (CheiralityException&) {
std::cout << "computeEP: Cheirality exception " << std::endl; std::cout << "whitenedError(E): Cheirality exception " << std::endl;
exit(EXIT_FAILURE); // TODO: throw exception exit(EXIT_FAILURE); // TODO: throw exception
} }
// if needed, whiten // if needed, whiten
for (size_t i = 0, row = 0; i < noise_.size(); i++, row += ZDim) { size_t m = keys_.size();
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Calculate error
const Z& zi = measured_.at(i);
Vector bi = (zi - predicted[i]).vector();
// if needed, whiten
SharedNoiseModel model = noise_.at(i); SharedNoiseModel model = noise_.at(i);
if (model) { if (model) {
// TODO: re-factor noiseModel to take any block/fixed vector/matrix // TODO: re-factor noiseModel to take any block/fixed vector/matrix
@ -272,9 +280,9 @@ public:
model->WhitenSystem(Ei, dummy); model->WhitenSystem(Ei, dummy);
E.block<ZDim, 3>(row, 0) = Ei; E.block<ZDim, 3>(row, 0) = Ei;
} }
b.segment<ZDim>(row) = bi;
} }
// Matrix PointCov; return b;
PointCov.noalias() = (E.transpose() * E).inverse();
} }
/** /**
@ -286,25 +294,25 @@ public:
* is applied in the caller, but the derivatives are computed here. * is applied in the caller, but the derivatives are computed here.
*/ */
Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F, Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F,
Matrix& E, boost::optional<Matrix&> H = boost::none) const { Matrix& E, boost::optional<Matrix&> G = boost::none) const {
// Project into CameraSet, calculating derivatives // Project into CameraSet, calculating derivatives
std::vector<Z> predicted; std::vector<Z> predicted;
try { try {
predicted = cameras.project(point, F, E, H); predicted = cameras.project(point, F, E, G);
} catch (CheiralityException&) { } catch (CheiralityException&) {
std::cout << "computeJacobians: Cheirality exception " << std::endl; std::cout << "whitenedError(E,F): Cheirality exception " << std::endl;
exit(EXIT_FAILURE); // TODO: throw exception exit(EXIT_FAILURE); // TODO: throw exception
} }
// Calculate error and whiten derivatives if needed // Calculate error and whiten derivatives if needed
size_t numKeys = keys_.size(); size_t m = keys_.size();
Vector b(ZDim * numKeys); Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < numKeys; i++, row += ZDim) { for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Calculate error // Calculate error
const Z& zi = measured_.at(i); const Z& zi = measured_.at(i);
b.segment<ZDim>(row) = (zi - predicted[i]).vector(); Vector bi = (zi - predicted[i]).vector();
// if we have a sensor offset, correct camera derivatives // if we have a sensor offset, correct camera derivatives
if (body_P_sensor_) { if (body_P_sensor_) {
@ -322,66 +330,30 @@ public:
// TODO, refactor noiseModel so we can take blocks // TODO, refactor noiseModel so we can take blocks
Matrix Fi = F.block<ZDim, 6>(row, 0); Matrix Fi = F.block<ZDim, 6>(row, 0);
Matrix Ei = E.block<ZDim, 3>(row, 0); Matrix Ei = E.block<ZDim, 3>(row, 0);
Vector bi = b.segment<ZDim>(row); if (!G)
if (!H)
model->WhitenSystem(Fi, Ei, bi); model->WhitenSystem(Fi, Ei, bi);
else { else {
Matrix Hi = H->block<ZDim, D - 6>(row, 0); Matrix Gi = G->block<ZDim, D - 6>(row, 0);
model->WhitenSystem(Fi, Ei, Hi, bi); model->WhitenSystem(Fi, Ei, Gi, bi);
H->block<ZDim, D - 6>(row, 0) = Hi; G->block<ZDim, D - 6>(row, 0) = Gi;
} }
b.segment<ZDim>(row) = bi;
F.block<ZDim, 6>(row, 0) = Fi; F.block<ZDim, 6>(row, 0) = Fi;
E.block<ZDim, 3>(row, 0) = Ei; E.block<ZDim, 3>(row, 0) = Ei;
} }
b.segment<ZDim>(row) = bi;
} }
return b; return b;
} }
/** /// Computes Point Covariance P from E
* Compute F, E, and b (called below in both vanilla and SVD versions), where static Matrix3 PointCov(Matrix& E) {
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives return (E.transpose() * E).inverse();
* with respect to the point. The value of cameras/point are passed as parameters.
*/
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
Vector& b, const Cameras& cameras, const Point3& point) const {
// Project into Camera set and calculate derivatives
// TODO: if D==6 we optimize only camera pose. That is fairly hacky!
Matrix F, H;
using boost::none;
boost::optional<Matrix&> optionalH(H);
b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalH);
// Now calculate f and divide up the F derivatives into Fblocks
double f = 0.0;
size_t numKeys = keys_.size();
for (size_t i = 0, row = 0; i < numKeys; i++, row += ZDim) {
// Accumulate normalized error
f += b.segment<ZDim>(row).squaredNorm();
// Get piece of F and possibly H
Matrix2D Fi;
if (D == 6)
Fi << F.block<ZDim, 6>(row, 0);
else
Fi << F.block<ZDim, 6>(row, 0), H.block<ZDim, D - 6>(row, 0);
// Push it onto Fblocks
Fblocks.push_back(KeyMatrix2D(keys_[i], Fi));
}
return f;
} }
/// Version that computes PointCov, with optional lambda parameter /// Computes Point Covariance P, with lambda parameter
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E, static Matrix3 PointCov(Matrix& E, double lambda,
Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, bool diagonalDamping = false) {
double lambda = 0.0, bool diagonalDamping = false) const {
double f = computeJacobians(Fblocks, E, b, cameras, point);
// Point covariance inv(E'*E)
Matrix3 EtE = E.transpose() * E; Matrix3 EtE = E.transpose() * E;
if (diagonalDamping) { // diagonal of the hessian if (diagonalDamping) { // diagonal of the hessian
@ -394,47 +366,86 @@ public:
EtE(2, 2) += lambda; EtE(2, 2) += lambda;
} }
PointCov.noalias() = (EtE).inverse(); return (EtE).inverse();
}
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
const Point3& point) const {
whitenedError(cameras, point, E);
P = PointCov(E);
}
/**
* Compute F, E, and b (called below in both vanilla and SVD versions), where
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
* with respect to the point. The value of cameras/point are passed as parameters.
*/
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
Vector& b, const Cameras& cameras, const Point3& point) const {
// Project into Camera set and calculate derivatives
// TODO: if D==6 we optimize only camera pose. That is fairly hacky!
Matrix F, G;
using boost::none;
boost::optional<Matrix&> optionalG(G);
b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG);
// Now calculate f and divide up the F derivatives into Fblocks
double f = 0.0;
size_t m = keys_.size();
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Accumulate normalized error
f += b.segment<ZDim>(row).squaredNorm();
// Get piece of F and possibly G
Matrix2D Fi;
if (D == 6)
Fi << F.block<ZDim, 6>(row, 0);
else
Fi << F.block<ZDim, 6>(row, 0), G.block<ZDim, D - 6>(row, 0);
// Push it onto Fblocks
Fblocks.push_back(KeyMatrix2D(keys_[i], Fi));
}
return f; return f;
} }
/// Create BIG block-diagonal matrix F from Fblocks
static void FillDiagonalF(const std::vector<KeyMatrix2D>& Fblocks, Matrix& F) {
size_t m = Fblocks.size();
F.resize(ZDim * m, D * m);
F.setZero();
for (size_t i = 0; i < m; ++i)
F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second;
}
/** /**
* Compute F, E, and b, where F and E are the stacked derivatives * Compute F, E, and b, where F and E are the stacked derivatives
* with respect to the point. The value of cameras/point are passed as parameters. * with respect to the point. The value of cameras/point are passed as parameters.
*/ */
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, double computeJacobians(Matrix& F, Matrix& E, Vector& b,
const Cameras& cameras, const Point3& point, const Cameras& cameras, const Point3& point) const {
const double lambda = 0.0) const {
size_t numKeys = keys_.size();
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, double f = computeJacobians(Fblocks, E, b, cameras, point);
lambda); FillDiagonalF(Fblocks, F);
F = zeros(This::ZDim * numKeys, D * numKeys);
for (size_t i = 0; i < keys_.size(); ++i) {
F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
}
return f; return f;
} }
/// SVD version /// SVD version
double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull, double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const Point3& point, double lambda = Vector& b, const Cameras& cameras, const Point3& point) const {
0.0, bool diagonalDamping = false) const {
Matrix E; Matrix E;
Matrix3 PointCov; // useless double f = computeJacobians(Fblocks, E, b, cameras, point);
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
// Do SVD on A // Do SVD on A
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU); Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues(); Vector s = svd.singularValues();
// Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3); size_t m = this->keys_.size();
size_t numKeys = this->keys_.size(); // Enull = zeros(ZDim * m, ZDim * m - 3);
Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns
return f; return f;
} }
@ -443,16 +454,9 @@ public:
// TODO, there should not be a Matrix version, really // TODO, there should not be a Matrix version, really
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
const Cameras& cameras, const Point3& point) const { const Cameras& cameras, const Point3& point) const {
int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
F.resize(ZDim * numKeys, D * numKeys); FillDiagonalF(Fblocks, F);
F.setZero();
for (size_t i = 0; i < this->keys_.size(); ++i)
F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
return f; return f;
} }
@ -467,10 +471,9 @@ public:
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov;
Vector b; Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, double f = computeJacobians(Fblocks, E, b, cameras, point);
diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
#ifdef HESSIAN_BLOCKS #ifdef HESSIAN_BLOCKS
@ -478,8 +481,8 @@ public:
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
std::vector < Vector > gs(numKeys); std::vector < Vector > gs(numKeys);
sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); sparseSchurComplement(Fblocks, E, P, b, Gs, gs);
// schurComplement(Fblocks, E, PointCov, b, Gs, gs); // schurComplement(Fblocks, E, P, b, Gs, gs);
//std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); //std::vector < Matrix > Gs2(Gs.begin(), Gs.end());
//std::vector < Vector > gs2(gs.begin(), gs.end()); //std::vector < Vector > gs2(gs.begin(), gs.end());
@ -492,7 +495,7 @@ public:
dims.back() = 1; dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ... sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
augmentedHessian(numKeys, numKeys)(0, 0) = f; augmentedHessian(numKeys, numKeys)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
augmentedHessian); augmentedHessian);
@ -500,11 +503,11 @@ public:
} }
/** /**
* Do Schur complement, given Jacobian as F,E,PointCov. * Do Schur complement, given Jacobian as F,E,P.
* Slow version - works on full matrices * Slow version - works on full matrices
*/ */
void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E, void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix& PointCov, const Vector& b, const Matrix3& P, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const { /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
// Schur complement trick // Schur complement trick
// Gs = F' * F - F' * E * inv(E'*E) * E' * F // Gs = F' * F - F' * E * inv(E'*E) * E' * F
@ -514,16 +517,14 @@ public:
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
/// Compute Full F ???? /// Compute Full F ????
Matrix F = zeros(ZDim * numKeys, D * numKeys); Matrix F;
for (size_t i = 0; i < this->keys_.size(); ++i) FillDiagonalF(Fblocks, F);
F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
Matrix H(D * numKeys, D * numKeys); Matrix H(D * numKeys, D * numKeys);
Vector gs_vector; Vector gs_vector;
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
gs_vector.noalias() = F.transpose() gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
* (b - (E * (PointCov * (E.transpose() * b))));
// Populate Gs and gs // Populate Gs and gs
int GsCount2 = 0; int GsCount2 = 0;
@ -540,11 +541,11 @@ public:
} }
/** /**
* Do Schur complement, given Jacobian as F,E,PointCov, return SymmetricBlockMatrix * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
* Fast version - works on with sparsity * Fast version - works on with sparsity
*/ */
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const { /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick // Schur complement trick
// Gs = F' * F - F' * E * P * E' * F // Gs = F' * F - F' * E * P * E' * F
@ -581,11 +582,11 @@ public:
} }
/** /**
* Do Schur complement, given Jacobian as F,E,PointCov, return Gs/gs * Do Schur complement, given Jacobian as F,E,P, return Gs/gs
* Fast version - works on with sparsity * Fast version - works on with sparsity
*/ */
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const { /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
// Schur complement trick // Schur complement trick
// Gs = F' * F - F' * E * P * E' * F // Gs = F' * F - F' * E * P * E' * F
@ -634,7 +635,7 @@ public:
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian. * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/ */
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
const double f, const FastVector<Key> allKeys, const double f, const FastVector<Key> allKeys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const { /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick // Schur complement trick
@ -717,13 +718,10 @@ public:
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov;
Vector b; Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, double f = computeJacobians(Fblocks, E, b, cameras, point);
diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys,
augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
} }
/** /**
@ -734,8 +732,8 @@ public:
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f( typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
new RegularImplicitSchurFactor<D>()); new RegularImplicitSchurFactor<D>());
computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point);
cameras, point, lambda, diagonalDamping); f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping);
f->initKeys(); f->initKeys();
return f; return f;
} }
@ -748,16 +746,15 @@ public:
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov;
Vector b; Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, computeJacobians(Fblocks, E, b, cameras, point);
diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov, return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, P, b);
b);
} }
/** /**
* Return Jacobians as JacobianFactor * Return Jacobians as JacobianFactor
* TODO lambda is currently ignored
*/ */
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const { const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
@ -765,7 +762,7 @@ public:
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
Vector b; Vector b;
Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
return boost::make_shared<JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); return boost::make_shared<JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
} }

96
gtsam/slam/SmartProjectionFactor.h
View File

@ -61,7 +61,8 @@ enum LinearizationMode {
* SmartProjectionFactor: triangulates point and keeps an estimate of it around. * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
*/ */
template<class CALIBRATION, size_t D> template<class CALIBRATION, size_t D>
class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>, D> { class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>,
D> {
protected: protected:
// Some triangulation parameters // Some triangulation parameters
@ -125,14 +126,14 @@ public:
const bool manageDegeneracy, const bool enableEPI, const bool manageDegeneracy, const bool enableEPI,
boost::optional<Pose3> body_P_sensor = boost::none, boost::optional<Pose3> body_P_sensor = boost::none,
double landmarkDistanceThreshold = 1e10, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : SmartFactorStatePtr(new SmartProjectionFactorState())) :
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state), false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
landmarkDistanceThreshold_(landmarkDistanceThreshold), landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { dynamicOutlierRejectionThreshold) {
} }
/** Virtual destructor */ /** Virtual destructor */
@ -249,11 +250,11 @@ public:
// Check landmark distance and reprojection errors to avoid outliers // Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0; double totalReprojError = 0.0;
size_t i=0; size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) { BOOST_FOREACH(const Camera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation(); Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away // we discard smart factors corresponding to points that are far away
if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
degenerate_ = true; degenerate_ = true;
break; break;
} }
@ -267,8 +268,8 @@ public:
i += 1; i += 1;
} }
// we discard smart factors that have large reprojection error // we discard smart factors that have large reprojection error
if(dynamicOutlierRejectionThreshold_ > 0 && if (dynamicOutlierRejectionThreshold_ > 0
totalReprojError/m > dynamicOutlierRejectionThreshold_) && totalReprojError / m > dynamicOutlierRejectionThreshold_)
degenerate_ = true; degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) { } catch (TriangulationUnderconstrainedException&) {
@ -297,7 +298,8 @@ public:
|| (!this->manageDegeneracy_ || (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) { && (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) { if (isDebug) {
std::cout << "createRegularImplicitSchurFactor: degenerate configuration" std::cout
<< "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl; << std::endl;
} }
return false; return false;
@ -318,9 +320,9 @@ public:
bool isDebug = false; bool isDebug = false;
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors // Create structures for Hessian Factors
std::vector < Key > js; std::vector<Key> js;
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector < Vector > gs(numKeys); std::vector<Vector> gs(numKeys);
if (this->measured_.size() != cameras.size()) { if (this->measured_.size() != cameras.size()) {
std::cout std::cout
@ -370,9 +372,8 @@ public:
// ================================================================== // ==================================================================
Matrix F, E; Matrix F, E;
Matrix3 PointCov;
Vector b; Vector b;
double f = computeJacobians(F, E, PointCov, b, cameras, lambda); double f = computeJacobians(F, E, b, cameras);
// Schur complement trick // Schur complement trick
// Frank says: should be possible to do this more efficiently? // Frank says: should be possible to do this more efficiently?
@ -380,20 +381,20 @@ public:
Matrix H(D * numKeys, D * numKeys); Matrix H(D * numKeys, D * numKeys);
Vector gs_vector; Vector gs_vector;
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); Matrix3 P = Base::PointCov(E, lambda);
gs_vector.noalias() = F.transpose() H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
* (b - (E * (PointCov * (E.transpose() * b)))); gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
if (isDebug) if (isDebug)
std::cout << "gs_vector size " << gs_vector.size() << std::endl; std::cout << "gs_vector size " << gs_vector.size() << std::endl;
// Populate Gs and gs // Populate Gs and gs
int GsCount2 = 0; int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D; DenseIndex i1D = i1 * D;
gs.at(i1) = gs_vector.segment < D > (i1D); gs.at(i1) = gs_vector.segment<D>(i1D);
for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
if (i2 >= i1) { if (i2 >= i1) {
Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
GsCount2++; GsCount2++;
} }
} }
@ -422,7 +423,7 @@ public:
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda); return Base::createJacobianQFactor(cameras, point_, lambda);
else else
return boost::make_shared< JacobianFactorQ<D, 2> >(this->keys_); return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
} }
/// Create a factor, takes values /// Create a factor, takes values
@ -434,16 +435,16 @@ public:
if (nonDegenerate) if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda); return createJacobianQFactor(myCameras, lambda);
else else
return boost::make_shared< JacobianFactorQ<D, 2> >(this->keys_); return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
} }
/// different (faster) way to compute Jacobian factor /// different (faster) way to compute Jacobian factor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda); return Base::createJacobianSVDFactor(cameras, point_, lambda);
else else
return boost::make_shared< JacobianFactorSVD<D, 2> >(this->keys_); return boost::make_shared<JacobianFactorSVD<D, 2> >(this->keys_);
} }
/// Returns true if nonDegenerate /// Returns true if nonDegenerate
@ -476,27 +477,27 @@ public:
return true; return true;
} }
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const {
return Base::computeEP(E, P, cameras, point_);
}
/// Takes values /// Takes values
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { bool computeEP(Matrix& E, Matrix& P, const Values& values) const {
Cameras myCameras; Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate) if (nonDegenerate)
computeEP(E, PointCov, myCameras); computeEP(E, P, myCameras);
return nonDegenerate; return nonDegenerate;
} }
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
return Base::computeEP(E, PointCov, cameras, point_);
}
/// Version that takes values, and creates the point /// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks, bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { Matrix& E, Vector& b, const Values& values) const {
Cameras myCameras; Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate) if (nonDegenerate)
computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); computeJacobians(Fblocks, E, b, myCameras);
return nonDegenerate; return nonDegenerate;
} }
@ -535,7 +536,7 @@ public:
this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
f += bi.squaredNorm(); f += bi.squaredNorm();
Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
E.block < 2, 2 > (2 * i, 0) = Ei; E.block<2, 2>(2 * i, 0) = Ei;
subInsert(b, bi, 2 * i); subInsert(b, bi, 2 * i);
} }
return f; return f;
@ -545,19 +546,6 @@ public:
} // end else } // end else
} }
/// Version that computes PointCov, with optional lambda parameter
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
const double lambda = 0.0) const {
double f = computeJacobians(Fblocks, E, b, cameras);
// Point covariance inv(E'*E)
PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
return f;
}
/// takes values /// takes values
bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks, bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Values& values) const { Matrix& Enull, Vector& b, const Values& values) const {
@ -582,9 +570,9 @@ public:
} }
/// Returns Matrix, TODO: maybe should not exist -> not sparse ! /// Returns Matrix, TODO: maybe should not exist -> not sparse !
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, double computeJacobians(Matrix& F, Matrix& E, Vector& b,
const Cameras& cameras, const double lambda) const { const Cameras& cameras) const {
return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); return Base::computeJacobians(F, E, b, cameras, point_);
} }
/// Calculate vector of re-projection errors, before applying noise model /// Calculate vector of re-projection errors, before applying noise model

29
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -407,9 +407,8 @@ public:
// ================================================================== // ==================================================================
Matrix F, E; Matrix F, E;
Matrix3 PointCov;
Vector b; Vector b;
double f = computeJacobians(F, E, PointCov, b, cameras, lambda); double f = computeJacobians(F, E, b, cameras);
// Schur complement trick // Schur complement trick
// Frank says: should be possible to do this more efficiently? // Frank says: should be possible to do this more efficiently?
@ -417,9 +416,9 @@ public:
Matrix H(D * numKeys, D * numKeys); Matrix H(D * numKeys, D * numKeys);
Vector gs_vector; Vector gs_vector;
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); Matrix3 P = Base::PointCov(E,lambda);
gs_vector.noalias() = F.transpose() H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
* (b - (E * (PointCov * (E.transpose() * b)))); gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl;
if (isDebug) std::cout << "H:\n" << H << std::endl; if (isDebug) std::cout << "H:\n" << H << std::endl;
@ -514,6 +513,11 @@ public:
return true; return true;
} }
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
Base::computeEP(E, PointCov, cameras, point_);
}
/// Takes values /// Takes values
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
Cameras myCameras; Cameras myCameras;
@ -523,18 +527,13 @@ public:
return nonDegenerate; return nonDegenerate;
} }
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
return Base::computeEP(E, PointCov, cameras, point_);
}
/// Version that takes values, and creates the point /// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks, bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { Matrix& E, Vector& b, const Values& values) const {
Cameras myCameras; Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate) if (nonDegenerate)
computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); computeJacobians(Fblocks, E, b, myCameras, 0.0);
return nonDegenerate; return nonDegenerate;
} }
@ -620,9 +619,9 @@ public:
// } // }
/// Returns Matrix, TODO: maybe should not exist -> not sparse ! /// Returns Matrix, TODO: maybe should not exist -> not sparse !
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, double computeJacobians(Matrix& F, Matrix& E, Vector& b,
const Cameras& cameras, const double lambda) const { const Cameras& cameras) const {
return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); return Base::computeJacobians(F, E, b, cameras, point_);
} }
/// Calculate vector of re-projection errors, before applying noise model /// Calculate vector of re-projection errors, before applying noise model