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

Made work with Unit3

release/4.3a0
dellaert 2015-03-05 23:22:24 -08:00
parent 6c72c29a56
commit 861ee8fef3
2 changed files with 53 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
gtsam/slam/SmartFactorBase.h
View File

@ -205,55 +205,39 @@ public:
return e && Base::equals(p, tol) && areMeasurementsEqual; return e && Base::equals(p, tol) && areMeasurementsEqual;
} }
/// Compute reprojection errors /**
Vector reprojectionError(const Cameras& cameras, const Point3& point) const { * Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives
return cameras.reprojectionError(point, measured_); */
template<class POINT>
Vector unwhitenedError(const Cameras& cameras, const POINT& point,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
return cameras.reprojectionError(point, measured_, Fs, E);
} }
/** /**
* Compute reprojection errors and derivatives * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
* Noise model applied
*/ */
Vector reprojectionError(const Cameras& cameras, const Point3& point, template<class POINT>
typename Cameras::FBlocks& F, Matrix& E) const { Vector whitenedError(const Cameras& cameras, const POINT& point) const {
return cameras.reprojectionError(point, measured_, F, E); Vector e = cameras.reprojectionError(point, measured_);
}
/// Calculate vector of re-projection errors, noise model applied
Vector whitenedErrors(const Cameras& cameras, const Point3& point) const {
Vector b = cameras.reprojectionError(point, measured_);
if (noiseModel_) if (noiseModel_)
noiseModel_->whitenInPlace(b); noiseModel_->whitenInPlace(e);
return b; return e;
}
/// Calculate vector of re-projection errors, noise model applied
// TODO: Unit3
Vector whitenedErrorsAtInfinity(const Cameras& cameras,
const Point3& point) const {
Vector b = cameras.reprojectionErrorAtInfinity(point, measured_);
if (noiseModel_)
noiseModel_->whitenInPlace(b);
return b;
} }
/** Calculate the error of the factor. /** Calculate the error of the factor.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
* This is different from reprojectionError(cameras,point) as each point is whitened * Will be used in "error(Values)" function required by NonlinearFactor base class
*/ */
template<class POINT>
double totalReprojectionError(const Cameras& cameras, double totalReprojectionError(const Cameras& cameras,
const Point3& point) const { const POINT& point) const {
Vector b = whitenedErrors(cameras, point); Vector e = whitenedError(cameras, point);
return 0.5 * b.dot(b); return 0.5 * e.dot(e);
}
/// Version for infinity
// TODO: Unit3
double totalReprojectionErrorAtInfinity(const Cameras& cameras,
const Point3& point) const {
Vector b = whitenedErrorsAtInfinity(cameras, point);
return 0.5 * b.dot(b);
} }
/// Computes Point Covariance P from E /// Computes Point Covariance P from E
@ -283,53 +267,49 @@ public:
* 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.
* TODO: Kill this obsolete method * TODO: Kill this obsolete method
*/ */
double computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b, template<class POINT>
const Cameras& cameras, const Point3& point) const { void computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras, const POINT& point) const {
// Project into Camera set and calculate derivatives // Project into Camera set and calculate derivatives
b = reprojectionError(cameras, point, Fblocks, E); // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
return b.squaredNorm(); // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
// = |A*dx - (z-h(x_bar))|
b = -unwhitenedError(cameras, point, Fblocks, E);
} }
/// SVD version /// SVD version
double computeJacobiansSVD(std::vector<MatrixZD>& Fblocks, Matrix& Enull, template<class POINT>
Vector& b, const Cameras& cameras, const Point3& point) const { void computeJacobiansSVD(std::vector<MatrixZD>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const POINT& point) const {
Matrix E; Matrix E;
double f = computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
// 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();
size_t m = this->keys_.size(); size_t m = this->keys_.size();
Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
return f;
} }
/** /**
* Linearize returns a Hessianfactor that is an approximation of error(p) * Linearize to a Hessianfactor
*/ */
boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor( boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
const Cameras& cameras, const Point3& point, const double lambda = 0.0, const Cameras& cameras, const Point3& point, const double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
int m = this->keys_.size();
std::vector<MatrixZD> Fblocks; std::vector<MatrixZD> Fblocks;
Matrix E; Matrix E;
Vector b; Vector b;
double f = computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix P = PointCov(E, lambda, diagonalDamping);
// Create a SymmetricBlockMatrix
size_t M1 = Dim * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, Dim);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// build augmented hessian // build augmented hessian
CameraSet<CAMERA>::SchurComplement(Fblocks, E, P, b, augmentedHessian); SymmetricBlockMatrix augmentedHessian = CameraSet<CAMERA>::SchurComplement(
augmentedHessian(m, m)(0, 0) = f; Fblocks, E, P, b);
return boost::make_shared<RegularHessianFactor<Dim> >(keys_, return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian); augmentedHessian);
@ -348,7 +328,7 @@ public:
Vector b; Vector b;
std::vector<MatrixZD> Fblocks; std::vector<MatrixZD> Fblocks;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix P = PointCov(E, lambda, diagonalDamping);
CameraSet<CAMERA>::UpdateSchurComplement(Fblocks, E, P, b, allKeys, keys_, CameraSet<CAMERA>::UpdateSchurComplement(Fblocks, E, P, b, allKeys, keys_,
augmentedHessian); augmentedHessian);
} }
@ -372,7 +352,7 @@ public:
std::vector<MatrixZD> F; std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
whitenJacobians(F, E, b); whitenJacobians(F, E, b);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix P = PointCov(E, lambda, diagonalDamping);
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E, return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
P, b); P, b);
} }
@ -388,7 +368,7 @@ public:
std::vector<MatrixZD> F; std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
const size_t M = b.size(); const size_t M = b.size();
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix P = PointCov(E, lambda, diagonalDamping);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n); return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
} }

35
gtsam/slam/SmartProjectionFactor.h
View File

@ -276,13 +276,13 @@ public:
// ================================================================== // ==================================================================
Matrix F, E; Matrix F, E;
Vector b; Vector b;
double f;
{ {
std::vector<typename Base::MatrixZD> Fblocks; std::vector<typename Base::MatrixZD> Fblocks;
f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
Base::whitenJacobians(Fblocks, E, b); Base::whitenJacobians(Fblocks, E, b);
Base::FillDiagonalF(Fblocks, F); // expensive ! Base::FillDiagonalF(Fblocks, F); // expensive !
} }
double f = b.squaredNorm();
// 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?
@ -373,30 +373,18 @@ public:
/// Compute F, E only (called below in both vanilla and SVD versions) /// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
double computeJacobiansWithTriangulatedPoint( void computeJacobiansWithTriangulatedPoint(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b, std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const { const Cameras& cameras) const {
if (!result_) { if (!result_) {
// TODO Luca clarify whether this works or not // Handle degeneracy
result_ = TriangulationResult( // TODO check flag whether we should do this
cameras[0].backprojectPointAtInfinity(this->measured_.at(0))); Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured_.at(0));
// TODO replace all this by Call to CameraSet Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
int m = this->keys_.size();
E = zeros(2 * m, 2);
b = zero(2 * m);
for (size_t i = 0; i < this->measured_.size(); i++) {
Matrix Fi, Ei;
Vector bi = -(cameras[i].projectPointAtInfinity(*result_, Fi, Ei)
- this->measured_.at(i)).vector();
Fblocks.push_back(Fi);
E.block<2, 2>(2 * i, 0) = Ei;
subInsert(b, bi, 2 * i);
}
return b.squaredNorm();
} else { } else {
// valid result: just return Base version // valid result: just return Base version
return Base::computeJacobians(Fblocks, E, b, cameras, *result_); Base::computeJacobians(Fblocks, E, b, cameras, *result_);
} }
} }
@ -427,7 +415,7 @@ public:
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
return Base::reprojectionError(cameras, *result_); return Base::unwhitenedError(cameras, *result_);
else else
return zero(cameras.size() * 2); return zero(cameras.size() * 2);
} }
@ -452,9 +440,8 @@ public:
else if (manageDegeneracy_) { else if (manageDegeneracy_) {
// Otherwise, manage the exceptions with rotation-only factors // Otherwise, manage the exceptions with rotation-only factors
const Point2& z0 = this->measured_.at(0); const Point2& z0 = this->measured_.at(0);
result_ = TriangulationResult( Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);
cameras.front().backprojectPointAtInfinity(z0)); return Base::totalReprojectionError(cameras, backprojected);
return Base::totalReprojectionErrorAtInfinity(cameras, *result_);
} else } else
// if we don't want to manage the exceptions we discard the factor // if we don't want to manage the exceptions we discard the factor
return 0.0; return 0.0;