diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b5ed39c55..a2bdfc059 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -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 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(row, 0) = Ei; } + b.segment(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 H = boost::none) const { + Matrix& E, boost::optional G = boost::none) const { // Project into CameraSet, calculating derivatives std::vector 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(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(row, 0); Matrix Ei = E.block(row, 0); - Vector bi = b.segment(row); - if (!H) + if (!G) model->WhitenSystem(Fi, Ei, bi); else { - Matrix Hi = H->block(row, 0); - model->WhitenSystem(Fi, Ei, Hi, bi); - H->block(row, 0) = Hi; + Matrix Gi = G->block(row, 0); + model->WhitenSystem(Fi, Ei, Gi, bi); + G->block(row, 0) = Gi; } - b.segment(row) = bi; F.block(row, 0) = Fi; E.block(row, 0) = Ei; } + b.segment(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& 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 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(row).squaredNorm(); - - // Get piece of F and possibly H - Matrix2D Fi; - if (D == 6) - Fi << F.block(row, 0); - else - Fi << F.block(row, 0), H.block(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& 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& 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 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(row).squaredNorm(); + + // Get piece of F and possibly G + Matrix2D Fi; + if (D == 6) + Fi << F.block(row, 0); + else + Fi << F.block(row, 0), G.block(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& 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 * 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 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 * 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& 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 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 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 * 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 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 (i1,i2) = ... + sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... augmentedHessian(numKeys, numKeys)(0, 0) = f; return boost::make_shared >(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& Fblocks, const Matrix& E, - const Matrix& PointCov, const Vector& b, + const Matrix3& P, const Vector& b, /*output ->*/std::vector& Gs, std::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 * 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& 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& 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& Gs, std::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& 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 allKeys, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick @@ -717,13 +718,10 @@ public: std::vector 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 (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 (i1,i2) = ... } /** @@ -734,8 +732,8 @@ public: bool diagonalDamping = false) const { typename boost::shared_ptr > f( new RegularImplicitSchurFactor()); - 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 Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, - b); + computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(Fblocks, E, P, b); } /** * Return Jacobians as JacobianFactor + * TODO lambda is currently ignored */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { @@ -765,7 +762,7 @@ public: std::vector 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 >(Fblocks, Enull, b); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index e763b3b02..7fb43152a 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -61,7 +61,8 @@ enum LinearizationMode { * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ template -class SmartProjectionFactor: public SmartFactorBase, D> { +class SmartProjectionFactor: public SmartFactorBase, + D> { protected: // Some triangulation parameters @@ -125,14 +126,14 @@ public: const bool manageDegeneracy, const bool enableEPI, boost::optional 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 js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::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(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(i1D, i2 * D); GsCount2++; } } @@ -422,7 +423,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// Create a factor, takes values @@ -434,16 +435,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared >(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& 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& 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& 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 diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 2fa23d09f..11513d19f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -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& 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