Isolated calculation of Point Covariance in static method(s) PointCov
dellaert
parent
4badb4a10f
commit
843bc7cb82
|
|
@ -245,12 +245,12 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Compute the matrices E and PointCov = inv(E'*E), where E stacks the ZDim*3 derivatives
|
||||
* of project with respect to the point, given each of the m cameras.
|
||||
* Assumes non-degenerate !
|
||||
* Compute whitenedError, returning only the derivative E, i.e.,
|
||||
* the stacked ZDim*3 derivatives of project with respect to the point.
|
||||
* Assumes non-degenerate ! TODO explain this
|
||||
*/
|
||||
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras,
|
||||
const Point3& point) const {
|
||||
Vector whitenedError(const Cameras& cameras, const Point3& point,
|
||||
Matrix& E) const {
|
||||
|
||||
// Project into CameraSet, calculating derivatives
|
||||
std::vector<Z> predicted;
|
||||
|
|
@ -258,12 +258,20 @@ public:
|
|||
using boost::none;
|
||||
predicted = cameras.project(point, none, E, none);
|
||||
} catch (CheiralityException&) {
|
||||
std::cout << "computeEP: Cheirality exception " << std::endl;
|
||||
std::cout << "whitenedError(E): Cheirality exception " << std::endl;
|
||||
exit(EXIT_FAILURE); // TODO: throw exception
|
||||
}
|
||||
|
||||
// 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);
|
||||
if (model) {
|
||||
// TODO: re-factor noiseModel to take any block/fixed vector/matrix
|
||||
|
|
@ -272,9 +280,9 @@ public:
|
|||
model->WhitenSystem(Ei, dummy);
|
||||
E.block<ZDim, 3>(row, 0) = Ei;
|
||||
}
|
||||
b.segment<ZDim>(row) = bi;
|
||||
}
|
||||
// Matrix PointCov;
|
||||
PointCov.noalias() = (E.transpose() * E).inverse();
|
||||
return b;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -286,25 +294,25 @@ public:
|
|||
* is applied in the caller, but the derivatives are computed here.
|
||||
*/
|
||||
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
|
||||
std::vector<Z> predicted;
|
||||
try {
|
||||
predicted = cameras.project(point, F, E, H);
|
||||
predicted = cameras.project(point, F, E, G);
|
||||
} catch (CheiralityException&) {
|
||||
std::cout << "computeJacobians: Cheirality exception " << std::endl;
|
||||
std::cout << "whitenedError(E,F): Cheirality exception " << std::endl;
|
||||
exit(EXIT_FAILURE); // TODO: throw exception
|
||||
}
|
||||
|
||||
// Calculate error and whiten derivatives if needed
|
||||
size_t numKeys = keys_.size();
|
||||
Vector b(ZDim * numKeys);
|
||||
for (size_t i = 0, row = 0; i < numKeys; 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);
|
||||
b.segment<ZDim>(row) = (zi - predicted[i]).vector();
|
||||
Vector bi = (zi - predicted[i]).vector();
|
||||
|
||||
// if we have a sensor offset, correct camera derivatives
|
||||
if (body_P_sensor_) {
|
||||
|
|
@ -322,66 +330,30 @@ public:
|
|||
// TODO, refactor noiseModel so we can take blocks
|
||||
Matrix Fi = F.block<ZDim, 6>(row, 0);
|
||||
Matrix Ei = E.block<ZDim, 3>(row, 0);
|
||||
Vector bi = b.segment<ZDim>(row);
|
||||
if (!H)
|
||||
if (!G)
|
||||
model->WhitenSystem(Fi, Ei, bi);
|
||||
else {
|
||||
Matrix Hi = H->block<ZDim, D - 6>(row, 0);
|
||||
model->WhitenSystem(Fi, Ei, Hi, bi);
|
||||
H->block<ZDim, D - 6>(row, 0) = Hi;
|
||||
Matrix Gi = G->block<ZDim, D - 6>(row, 0);
|
||||
model->WhitenSystem(Fi, Ei, Gi, bi);
|
||||
G->block<ZDim, D - 6>(row, 0) = Gi;
|
||||
}
|
||||
b.segment<ZDim>(row) = bi;
|
||||
F.block<ZDim, 6>(row, 0) = Fi;
|
||||
E.block<ZDim, 3>(row, 0) = Ei;
|
||||
}
|
||||
b.segment<ZDim>(row) = bi;
|
||||
}
|
||||
return b;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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, 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;
|
||||
/// Computes Point Covariance P from E
|
||||
static Matrix3 PointCov(Matrix& E) {
|
||||
return (E.transpose() * E).inverse();
|
||||
}
|
||||
|
||||
/// Version that computes PointCov, with optional lambda parameter
|
||||
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
|
||||
Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point,
|
||||
double lambda = 0.0, bool diagonalDamping = false) const {
|
||||
/// Computes Point Covariance P, with lambda parameter
|
||||
static Matrix3 PointCov(Matrix& E, double lambda,
|
||||
bool diagonalDamping = false) {
|
||||
|
||||
double f = computeJacobians(Fblocks, E, b, cameras, point);
|
||||
|
||||
// Point covariance inv(E'*E)
|
||||
Matrix3 EtE = E.transpose() * E;
|
||||
|
||||
if (diagonalDamping) { // diagonal of the hessian
|
||||
|
|
@ -394,47 +366,86 @@ public:
|
|||
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;
|
||||
}
|
||||
|
||||
/// 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
|
||||
* with respect to the point. The value of cameras/point are passed as parameters.
|
||||
*/
|
||||
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
|
||||
const Cameras& cameras, const Point3& point,
|
||||
const double lambda = 0.0) const {
|
||||
|
||||
size_t numKeys = keys_.size();
|
||||
double computeJacobians(Matrix& F, Matrix& E, Vector& b,
|
||||
const Cameras& cameras, const Point3& point) const {
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
|
||||
lambda);
|
||||
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
|
||||
}
|
||||
double f = computeJacobians(Fblocks, E, b, cameras, point);
|
||||
FillDiagonalF(Fblocks, F);
|
||||
return f;
|
||||
}
|
||||
|
||||
/// SVD version
|
||||
double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
|
||||
Vector& b, const Cameras& cameras, const Point3& point, double lambda =
|
||||
0.0, bool diagonalDamping = false) const {
|
||||
Vector& b, const Cameras& cameras, const Point3& point) const {
|
||||
|
||||
Matrix E;
|
||||
Matrix3 PointCov; // useless
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
|
||||
diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
|
||||
double f = computeJacobians(Fblocks, E, b, cameras, point);
|
||||
|
||||
// Do SVD on A
|
||||
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
|
||||
Vector s = svd.singularValues();
|
||||
// Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3);
|
||||
size_t numKeys = this->keys_.size();
|
||||
Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns
|
||||
size_t m = this->keys_.size();
|
||||
// Enull = zeros(ZDim * m, ZDim * m - 3);
|
||||
Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns
|
||||
|
||||
return f;
|
||||
}
|
||||
|
|
@ -443,16 +454,9 @@ public:
|
|||
// TODO, there should not be a Matrix version, really
|
||||
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
|
||||
const Cameras& cameras, const Point3& point) const {
|
||||
|
||||
int numKeys = this->keys_.size();
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
|
||||
F.resize(ZDim * numKeys, D * numKeys);
|
||||
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
|
||||
|
||||
FillDiagonalF(Fblocks, F);
|
||||
return f;
|
||||
}
|
||||
|
||||
|
|
@ -467,10 +471,9 @@ public:
|
|||
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
Matrix E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
|
||||
diagonalDamping);
|
||||
double f = computeJacobians(Fblocks, E, b, cameras, point);
|
||||
Matrix3 P = PointCov(E, lambda, diagonalDamping);
|
||||
|
||||
//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
|
||||
#ifdef HESSIAN_BLOCKS
|
||||
|
|
@ -478,8 +481,8 @@ public:
|
|||
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector < Vector > gs(numKeys);
|
||||
|
||||
sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs);
|
||||
// schurComplement(Fblocks, E, PointCov, b, Gs, gs);
|
||||
sparseSchurComplement(Fblocks, E, P, b, Gs, gs);
|
||||
// schurComplement(Fblocks, E, P, b, Gs, gs);
|
||||
|
||||
//std::vector < Matrix > Gs2(Gs.begin(), Gs.end());
|
||||
//std::vector < Vector > gs2(gs.begin(), gs.end());
|
||||
|
|
@ -492,7 +495,7 @@ public:
|
|||
dims.back() = 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;
|
||||
return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
|
||||
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
|
||||
*/
|
||||
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 {
|
||||
// Schur complement trick
|
||||
// Gs = F' * F - F' * E * inv(E'*E) * E' * F
|
||||
|
|
@ -514,16 +517,14 @@ public:
|
|||
int numKeys = this->keys_.size();
|
||||
|
||||
/// Compute Full F ????
|
||||
Matrix F = zeros(ZDim * numKeys, D * numKeys);
|
||||
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
|
||||
Matrix F;
|
||||
FillDiagonalF(Fblocks, F);
|
||||
|
||||
Matrix H(D * numKeys, D * numKeys);
|
||||
Vector gs_vector;
|
||||
|
||||
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
|
||||
gs_vector.noalias() = F.transpose()
|
||||
* (b - (E * (PointCov * (E.transpose() * b))));
|
||||
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
|
||||
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
|
||||
|
||||
// Populate Gs and gs
|
||||
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
|
||||
*/
|
||||
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 {
|
||||
// Schur complement trick
|
||||
// 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
|
||||
*/
|
||||
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 {
|
||||
// Schur complement trick
|
||||
// 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.
|
||||
*/
|
||||
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,
|
||||
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
|
||||
// Schur complement trick
|
||||
|
|
@ -717,13 +718,10 @@ public:
|
|||
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
Matrix E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
|
||||
diagonalDamping);
|
||||
|
||||
updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys,
|
||||
augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
|
||||
double f = computeJacobians(Fblocks, E, b, cameras, point);
|
||||
Matrix3 P = PointCov(E, lambda, diagonalDamping);
|
||||
updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -734,8 +732,8 @@ public:
|
|||
bool diagonalDamping = false) const {
|
||||
typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
|
||||
new RegularImplicitSchurFactor<D>());
|
||||
computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
|
||||
cameras, point, lambda, diagonalDamping);
|
||||
computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point);
|
||||
f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping);
|
||||
f->initKeys();
|
||||
return f;
|
||||
}
|
||||
|
|
@ -748,16 +746,15 @@ public:
|
|||
bool diagonalDamping = false) const {
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
Matrix E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
|
||||
diagonalDamping);
|
||||
return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov,
|
||||
b);
|
||||
computeJacobians(Fblocks, E, b, cameras, point);
|
||||
Matrix3 P = PointCov(E, lambda, diagonalDamping);
|
||||
return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, P, b);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return Jacobians as JacobianFactor
|
||||
* TODO lambda is currently ignored
|
||||
*/
|
||||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
|
||||
|
|
@ -765,7 +762,7 @@ public:
|
|||
std::vector<KeyMatrix2D> Fblocks;
|
||||
Vector b;
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -61,7 +61,8 @@ enum LinearizationMode {
|
|||
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
|
||||
*/
|
||||
template<class CALIBRATION, size_t D>
|
||||
class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>, D> {
|
||||
class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>,
|
||||
D> {
|
||||
protected:
|
||||
|
||||
// Some triangulation parameters
|
||||
|
|
@ -125,14 +126,14 @@ public:
|
|||
const bool manageDegeneracy, const bool enableEPI,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1,
|
||||
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
|
||||
double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
|
||||
SmartFactorStatePtr(new SmartProjectionFactorState())) :
|
||||
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
|
||||
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
|
||||
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
|
||||
false), verboseCheirality_(false), state_(state),
|
||||
landmarkDistanceThreshold_(landmarkDistanceThreshold),
|
||||
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
|
||||
false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
|
||||
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
|
||||
dynamicOutlierRejectionThreshold) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
|
|
@ -249,11 +250,11 @@ public:
|
|||
|
||||
// Check landmark distance and reprojection errors to avoid outliers
|
||||
double totalReprojError = 0.0;
|
||||
size_t i=0;
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(const Camera& camera, cameras) {
|
||||
Point3 cameraTranslation = camera.pose().translation();
|
||||
// we discard smart factors corresponding to points that are far away
|
||||
if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){
|
||||
if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
|
||||
degenerate_ = true;
|
||||
break;
|
||||
}
|
||||
|
|
@ -267,8 +268,8 @@ public:
|
|||
i += 1;
|
||||
}
|
||||
// we discard smart factors that have large reprojection error
|
||||
if(dynamicOutlierRejectionThreshold_ > 0 &&
|
||||
totalReprojError/m > dynamicOutlierRejectionThreshold_)
|
||||
if (dynamicOutlierRejectionThreshold_ > 0
|
||||
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
|
||||
degenerate_ = true;
|
||||
|
||||
} catch (TriangulationUnderconstrainedException&) {
|
||||
|
|
@ -297,7 +298,8 @@ public:
|
|||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_))) {
|
||||
if (isDebug) {
|
||||
std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
|
||||
std::cout
|
||||
<< "createRegularImplicitSchurFactor: degenerate configuration"
|
||||
<< std::endl;
|
||||
}
|
||||
return false;
|
||||
|
|
@ -318,9 +320,9 @@ public:
|
|||
bool isDebug = false;
|
||||
size_t numKeys = this->keys_.size();
|
||||
// Create structures for Hessian Factors
|
||||
std::vector < Key > js;
|
||||
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector < Vector > gs(numKeys);
|
||||
std::vector<Key> js;
|
||||
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector<Vector> gs(numKeys);
|
||||
|
||||
if (this->measured_.size() != cameras.size()) {
|
||||
std::cout
|
||||
|
|
@ -370,9 +372,8 @@ public:
|
|||
|
||||
// ==================================================================
|
||||
Matrix F, E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
double f = computeJacobians(F, E, PointCov, b, cameras, lambda);
|
||||
double f = computeJacobians(F, E, b, cameras);
|
||||
|
||||
// Schur complement trick
|
||||
// Frank says: should be possible to do this more efficiently?
|
||||
|
|
@ -380,20 +381,20 @@ public:
|
|||
Matrix H(D * numKeys, D * numKeys);
|
||||
Vector gs_vector;
|
||||
|
||||
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
|
||||
gs_vector.noalias() = F.transpose()
|
||||
* (b - (E * (PointCov * (E.transpose() * b))));
|
||||
Matrix3 P = Base::PointCov(E, lambda);
|
||||
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
|
||||
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
|
||||
if (isDebug)
|
||||
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
|
||||
|
||||
// Populate Gs and gs
|
||||
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;
|
||||
gs.at(i1) = gs_vector.segment < D > (i1D);
|
||||
for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) {
|
||||
gs.at(i1) = gs_vector.segment<D>(i1D);
|
||||
for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
|
||||
if (i2 >= i1) {
|
||||
Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D);
|
||||
Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
|
||||
GsCount2++;
|
||||
}
|
||||
}
|
||||
|
|
@ -422,7 +423,7 @@ public:
|
|||
if (triangulateForLinearize(cameras))
|
||||
return Base::createJacobianQFactor(cameras, point_, lambda);
|
||||
else
|
||||
return boost::make_shared< JacobianFactorQ<D, 2> >(this->keys_);
|
||||
return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
|
||||
}
|
||||
|
||||
/// Create a factor, takes values
|
||||
|
|
@ -434,16 +435,16 @@ public:
|
|||
if (nonDegenerate)
|
||||
return createJacobianQFactor(myCameras, lambda);
|
||||
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
|
||||
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
|
||||
double lambda) const {
|
||||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
return Base::createJacobianSVDFactor(cameras, point_, lambda);
|
||||
else
|
||||
return boost::make_shared< JacobianFactorSVD<D, 2> >(this->keys_);
|
||||
return boost::make_shared<JacobianFactorSVD<D, 2> >(this->keys_);
|
||||
}
|
||||
|
||||
/// Returns true if nonDegenerate
|
||||
|
|
@ -476,27 +477,27 @@ public:
|
|||
return true;
|
||||
}
|
||||
|
||||
/// Assumes non-degenerate !
|
||||
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const {
|
||||
return Base::computeEP(E, P, cameras, point_);
|
||||
}
|
||||
|
||||
/// Takes values
|
||||
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
|
||||
bool computeEP(Matrix& E, Matrix& P, const Values& values) const {
|
||||
Cameras myCameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (nonDegenerate)
|
||||
computeEP(E, PointCov, myCameras);
|
||||
computeEP(E, P, myCameras);
|
||||
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
|
||||
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;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (nonDegenerate)
|
||||
computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0);
|
||||
computeJacobians(Fblocks, E, b, myCameras);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
|
|
@ -535,7 +536,7 @@ public:
|
|||
this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
|
||||
f += bi.squaredNorm();
|
||||
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);
|
||||
}
|
||||
return f;
|
||||
|
|
@ -545,19 +546,6 @@ public:
|
|||
} // 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
|
||||
bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
Matrix& Enull, Vector& b, const Values& values) const {
|
||||
|
|
@ -582,9 +570,9 @@ public:
|
|||
}
|
||||
|
||||
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
|
||||
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
|
||||
const Cameras& cameras, const double lambda) const {
|
||||
return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda);
|
||||
double computeJacobians(Matrix& F, Matrix& E, Vector& b,
|
||||
const Cameras& cameras) const {
|
||||
return Base::computeJacobians(F, E, b, cameras, point_);
|
||||
}
|
||||
|
||||
/// Calculate vector of re-projection errors, before applying noise model
|
||||
|
|
|
|||
|
|
@ -407,9 +407,8 @@ public:
|
|||
|
||||
// ==================================================================
|
||||
Matrix F, E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
double f = computeJacobians(F, E, PointCov, b, cameras, lambda);
|
||||
double f = computeJacobians(F, E, b, cameras);
|
||||
|
||||
// Schur complement trick
|
||||
// Frank says: should be possible to do this more efficiently?
|
||||
|
|
@ -417,9 +416,9 @@ public:
|
|||
Matrix H(D * numKeys, D * numKeys);
|
||||
Vector gs_vector;
|
||||
|
||||
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
|
||||
gs_vector.noalias() = F.transpose()
|
||||
* (b - (E * (PointCov * (E.transpose() * b))));
|
||||
Matrix3 P = Base::PointCov(E,lambda);
|
||||
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
|
||||
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 << "H:\n" << H << std::endl;
|
||||
|
|
@ -514,6 +513,11 @@ public:
|
|||
return true;
|
||||
}
|
||||
|
||||
/// Assumes non-degenerate !
|
||||
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
|
||||
Base::computeEP(E, PointCov, cameras, point_);
|
||||
}
|
||||
|
||||
/// Takes values
|
||||
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
|
||||
Cameras myCameras;
|
||||
|
|
@ -523,18 +527,13 @@ public:
|
|||
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
|
||||
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;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (nonDegenerate)
|
||||
computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0);
|
||||
computeJacobians(Fblocks, E, b, myCameras, 0.0);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
|
|
@ -620,9 +619,9 @@ public:
|
|||
// }
|
||||
|
||||
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
|
||||
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
|
||||
const Cameras& cameras, const double lambda) const {
|
||||
return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda);
|
||||
double computeJacobians(Matrix& F, Matrix& E, Vector& b,
|
||||
const Cameras& cameras) const {
|
||||
return Base::computeJacobians(F, E, b, cameras, point_);
|
||||
}
|
||||
|
||||
/// Calculate vector of re-projection errors, before applying noise model
|
||||
|
|
|
|||
Loading…
Reference in New Issue