diff --git a/.cproject b/.cproject index 94e8c3a50..7111b0a6b 100644 --- a/.cproject +++ b/.cproject @@ -1309,6 +1309,7 @@ make + testSimulated2DOriented.run true false @@ -1348,6 +1349,7 @@ make + testSimulated2D.run true false @@ -1355,6 +1357,7 @@ make + testSimulated3D.run true false @@ -1458,7 +1461,6 @@ make - testErrors.run true false @@ -1536,10 +1538,10 @@ true true - + make - -j5 - testImplicitSchurFactor.run + -j4 + testRegularImplicitSchurFactor.run true true true @@ -1793,7 +1795,6 @@ cpack - -G DEB true false @@ -1801,7 +1802,6 @@ cpack - -G RPM true false @@ -1809,7 +1809,6 @@ cpack - -G TGZ true false @@ -1817,7 +1816,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2009,6 +2007,7 @@ make + tests/testGaussianISAM2 true false @@ -2160,7 +2159,6 @@ make - tests/testBayesTree.run true false @@ -2168,7 +2166,6 @@ make - testBinaryBayesNet.run true false @@ -2216,7 +2213,6 @@ make - testSymbolicBayesNet.run true false @@ -2224,7 +2220,6 @@ make - tests/testSymbolicFactor.run true false @@ -2232,7 +2227,6 @@ make - testSymbolicFactorGraph.run true false @@ -2248,7 +2242,6 @@ make - tests/testBayesTree true false @@ -3392,7 +3385,6 @@ make - testGraph.run true false @@ -3400,7 +3392,6 @@ make - testJunctionTree.run true false @@ -3408,7 +3399,6 @@ make - testSymbolicBayesNetB.run true false diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index 5de2b5f4d..1e5426f33 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -129,7 +129,7 @@ TEST(PinholeSet, Pinhole) { // Instantiate triangulateSafe TriangulationParameters params; TriangulationResult actual = set.triangulateSafe(z,params); - CHECK(actual.degenerate); + CHECK(actual.degenerate()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 15721e81c..a67f26bf2 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -316,12 +316,57 @@ struct TriangulationParameters { _landmarkDistanceThreshold), dynamicOutlierRejectionThreshold( _dynamicOutlierRejectionThreshold) { } + + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } }; -struct TriangulationResult { - Point3 point; - bool degenerate; - bool cheiralityException; +/** + * TriangulationResult is an optional point, along with the reasons why it is invalid. + */ +class TriangulationResult: public boost::optional { + enum Status { + VALID, DEGENERATE, BEHIND_CAMERA + }; + Status status_; + TriangulationResult(Status s) : + status_(s) { + } +public: + TriangulationResult(const Point3& p) : + status_(VALID) { + reset(p); + } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult BehindCamera() { + return TriangulationResult(BEHIND_CAMERA); + } + bool degenerate() const { + return status_ == DEGENERATE; + } + bool behindCamera() const { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult& result) { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; + } }; /// triangulateSafe: extensive checking of the outcome @@ -330,62 +375,53 @@ TriangulationResult triangulateSafe(const std::vector& cameras, const std::vector& measured, const TriangulationParameters& params) { - TriangulationResult result; - size_t m = cameras.size(); // if we have a single pose the corresponding factor is uninformative - if (m < 2) { - result.degenerate = true; - return result; - } + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); - // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - result.point = triangulatePoint3(cameras, measured, - params.rankTolerance, params.enableEPI); - result.degenerate = false; - result.cheiralityException = false; + // Check landmark distance and reprojection errors to avoid outliers + size_t i = 0; + double totalReprojError = 0.0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + // we discard smart factors corresponding to points that are far away + Point3 cameraTranslation = camera.pose().translation(); + if (cameraTranslation.distance(point) > params.landmarkDistanceThreshold) + return TriangulationResult::Degenerate(); + // Also flag if point is behind any of the cameras + try { + const Point2& zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + return TriangulationResult::BehindCamera(); + } + i += 1; + } + // we discard smart factors that have large reprojection error + if (params.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); - // Check landmark distance and reprojection errors to avoid outliers - double totalReprojError = 0.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(result.point) - > params.landmarkDistanceThreshold) { - result.degenerate = true; - break; - } - const Point2& zi = measured.at(i); - try { - Point2 reprojectionError(camera.project(result.point) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - result.cheiralityException = true; - } - i += 1; + // all good! + return TriangulationResult(point); + } catch (TriangulationUnderconstrainedException&) { + // if TriangulationUnderconstrainedException can be + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + // in the second case we want to use a rotation-only smart factor + return TriangulationResult::Degenerate(); + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint + return TriangulationResult::BehindCamera(); } - // we discard smart factors that have large reprojection error - if (params.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params.dynamicOutlierRejectionThreshold) - result.degenerate = true; - - } catch (TriangulationUnderconstrainedException&) { - // if TriangulationUnderconstrainedException can be - // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before - // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) - // in the second case we want to use a rotation-only smart factor - result.degenerate = true; - result.cheiralityException = false; - } catch (TriangulationCheiralityException&) { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers - // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint - result.cheiralityException = true; - } - return result; } } // \namespace gtsam diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 731db479f..88fb4b6e1 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -17,7 +18,7 @@ namespace gtsam { /** * RegularImplicitSchurFactor */ -template // +template // class RegularImplicitSchurFactor: public GaussianFactor { public: @@ -26,12 +27,12 @@ public: protected: - typedef Eigen::Matrix Matrix2D; ///< type of an F block + typedef Eigen::Matrix Matrix2D; ///< type of an F block typedef Eigen::Matrix MatrixDD; ///< camera hessian typedef std::pair KeyMatrix2D; ///< named F block - const std::vector Fblocks_; ///< All 2*D F blocks (one for each camera) - const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) + const std::vector Fblocks_; ///< All Z*D F blocks (one for each camera) + const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (Z*Z if degenerate) const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Vector b_; ///< 2m-dimensional RHS vector @@ -122,15 +123,141 @@ public: "RegularImplicitSchurFactor::jacobian non implemented"); return std::make_pair(Matrix(), Vector()); } - virtual Matrix augmentedInformation() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::augmentedInformation non implemented"); - return Matrix(); + + /** + * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix + * Fast version - works on with sparsity + */ + static void sparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) { + // Schur complement trick + // G = F' * F - F' * E * P * E' * F + // g = F' * (b - E * P * E' * b) + + // a single point is observed in m cameras + size_t m = Fblocks.size(); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera + + const Matrix2D& Fi = Fblocks.at(i).second; + const Matrix23 Ei_P = E.block(Z * i, 0) * P; + + // D = (Dx2) * (Z) + augmentedHessian(i, m) = Fi.transpose() * b.segment(Z * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian(i, i) = Fi.transpose() + * (Fi - Ei_P * E.block(Z * i, 0).transpose() * Fi); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Matrix2D& Fj = Fblocks.at(j).second; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian(i, j) = -Fi.transpose() + * (Ei_P * E.block(Z * j, 0).transpose() * Fj); + } + } // end of for over cameras } + + /** + * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, + * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. + */ + static void updateSparseSchurComplement( + const std::vector& Fblocks, const Matrix& E, + const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, + const FastVector& allKeys, const FastVector& keys, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) { + + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); + // Schur complement trick + // G = F' * F - F' * E * P * E' * F + // g = F' * (b - E * P * E' * b) + + MatrixDD matrixBlock; + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + + // a single point is observed in m cameras + size_t m = Fblocks.size(); // cameras observing current point + size_t aug_m = (augmentedHessian.rows() - 1) / D; // all cameras in the group + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera in the current factor + + const Matrix2D& Fi = Fblocks.at(i).second; + const Matrix23 Ei_P = E.block(Z * i, 0) * P; + + // D = (DxZDim) * (Z) + // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) + // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) + // Key cameraKey_i = this->keys_[i]; + DenseIndex aug_i = KeySlotMap.at(keys[i]); + + // information vector - store previous vector + // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, aug_m) = + augmentedHessian(aug_i, aug_m).knownOffDiagonal() + + Fi.transpose() * b.segment(Z * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + // main block diagonal - store previous block + matrixBlock = augmentedHessian(aug_i, aug_i); + // add contribution of current factor + augmentedHessian(aug_i, aug_i) = matrixBlock + + (Fi.transpose() + * (Fi - Ei_P * E.block(Z * i, 0).transpose() * Fi)); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Matrix2D& Fj = Fblocks.at(j).second; + + //Key cameraKey_j = this->keys_[j]; + DenseIndex aug_j = KeySlotMap.at(keys[j]); + + // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) + // off diagonal block - store previous block + // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, aug_j) = + augmentedHessian(aug_i, aug_j).knownOffDiagonal() + - Fi.transpose() + * (Ei_P * E.block(Z * j, 0).transpose() * Fj); + } + } // end of for over cameras + + augmentedHessian(aug_m, aug_m)(0, 0) += f; + } + + /// *Compute* full augmented information matrix + virtual Matrix augmentedInformation() const { + + // Create a SymmetricBlockMatrix + int m = this->keys_.size(); + size_t M1 = D * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Do the Schur complement + sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); + return augmentedHessian.matrix(); + } + + /// *Compute* full information matrix virtual Matrix information() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::information non implemented"); - return Matrix(); + Matrix augmented = augmentedInformation(); + int m = this->keys_.size(); + size_t M = D * m; + return augmented.block(0,0,M,M); } /// Return the diagonal of the Hessian for this factor @@ -142,10 +269,10 @@ public: Key j = keys_[pos]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) + // D x 3 = (D x Z) * (Z x 3) const Matrix2D& Fj = Fblocks_[pos].second; Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + * E_.block(Z * pos, 0); Eigen::Matrix dj; for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian @@ -174,10 +301,10 @@ public: Key j = keys_[pos]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) + // D x 3 = (D x Z) * (Z x 3) const Matrix2D& Fj = Fblocks_[pos].second; Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + * E_.block(Z * pos, 0); DVector dj; for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian @@ -195,28 +322,28 @@ public: // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) + // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) const Matrix2D& Fj = Fblocks_[pos].second; // Eigen::Matrix FtE = Fj.transpose() - // * E_.block<2, 3>(2 * pos, 0); + // * E_.block(Z * pos, 0); // blocks[j] = Fj.transpose() * Fj // - FtE * PointCovariance_ * FtE.transpose(); - const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); + const Matrix23& Ej = E_.block(Z * pos, 0); blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( - // static const Eigen::Matrix I2 = eye(2); + // static const Eigen::Matrix I2 = eye(Z); // Matrix2 Q = // - // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); + // I2 - E_.block(Z * pos, 0) * PointCovariance_ * E_.block(Z * pos, 0).transpose(); // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } virtual GaussianFactor::shared_ptr clone() const { - return boost::make_shared >(Fblocks_, + return boost::make_shared >(Fblocks_, PointCovariance_, E_, b_); throw std::runtime_error( "RegularImplicitSchurFactor::clone non implemented"); @@ -226,7 +353,7 @@ public: } virtual GaussianFactor::shared_ptr negate() const { - return boost::make_shared >(Fblocks_, + return boost::make_shared >(Fblocks_, PointCovariance_, E_, b_); throw std::runtime_error( "RegularImplicitSchurFactor::negate non implemented"); @@ -247,23 +374,23 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b) + * @brief Calculate corrected error Q*(e-Z*b) = (I - E*P*E')*(e-Z*b) */ void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m + // d1 = E.transpose() * (e1-Z*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block<2, 3>(2 * k, 0).transpose() - * (e1[k] - 2 * b_.segment<2>(k * 2)); + d1 += E_.block(Z * k, 0).transpose() + * (e1[k] - Z * b_.segment(k * Z)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - 2 * b_.segment<2>(k * 2) - E_.block<2, 3>(2 * k, 0) * d2; + e2[k] = e1[k] - Z * b_.segment(k * Z) - E_.block(Z * k, 0) * d2; } /* @@ -305,7 +432,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment<2>(k * 2); + e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment(k * Z); projectError(e1, e2); double result = 0; @@ -324,14 +451,14 @@ public: Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block<2, 3>(2 * k, 0).transpose() * e1[k]; + d1 += E_.block(Z * k, 0).transpose() * e1[k]; // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block<2, 3>(2 * k, 0) * d2; + e2[k] = e1[k] - E_.block(Z * k, 0) * d2; } /// Scratch space for multiplyHessianAdd @@ -426,7 +553,7 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment<2>(2 * k); + e1[k] = b_.segment(Z * k); projectError(e1, e2); // g = F.transpose()*e2 @@ -453,7 +580,7 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment<2>(2 * k); + e1[k] = b_.segment(Z * k); projectError(e1, e2); for (size_t k = 0; k < size(); ++k) { // for each camera in the factor @@ -472,8 +599,8 @@ public: // end class RegularImplicitSchurFactor // traits -template struct traits > : public Testable< - RegularImplicitSchurFactor > { +template struct traits > : public Testable< + RegularImplicitSchurFactor > { }; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 417ba1354..d1c0d76e2 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -337,14 +337,14 @@ public: double f = computeJacobians(Fblocks, E, b, cameras, point); Matrix3 P = PointCov(E, lambda, diagonalDamping); - // we create directly a SymmetricBlockMatrix + // Create a SymmetricBlockMatrix size_t M1 = Dim * m + 1; std::vector 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 - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); augmentedHessian(m, m)(0, 0) = f; @@ -352,121 +352,6 @@ public: augmentedHessian); } - /** - * 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 Matrix3& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - // Dim = (Dx2) * (2) - // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() - * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian(i1, i1) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); - - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i1, i2) = -Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - } - - /** - * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, - * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. - */ - void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, - const double f, const FastVector allKeys, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - MatrixDD matrixBlock; - typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix - - FastMap KeySlotMap; - for (size_t slot = 0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); // cameras observing current point - size_t aug_numKeys = (augmentedHessian.rows() - 1) / Dim; // all cameras in the group - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - // Dim = (DxZDim) * (ZDim) - // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) - // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) - // Key cameraKey_i1 = this->keys_[i1]; - DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]]; - - // information vector - store previous vector - // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, - aug_numKeys).knownOffDiagonal() - + Fi1.transpose() * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - // main block diagonal - store previous block - matrixBlock = augmentedHessian(aug_i1, aug_i1); - // add contribution of current factor - augmentedHessian(aug_i1, aug_i1) = - matrixBlock - + (Fi1.transpose() - * (Fi1 - - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1)); - - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - //Key cameraKey_i2 = this->keys_[i2]; - DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - - // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) - // off diagonal block - store previous block - // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i1, aug_i2) = - augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - - augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; - } - /** * Add the contribution of the smart factor to a pre-allocated Hessian, * using sparse linear algebra. More efficient than the creation of the @@ -476,33 +361,38 @@ public: const double lambda, bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian, const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; Matrix E; Vector b; + std::vector Fblocks; 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) = ... + RegularImplicitSchurFactor::updateSparseSchurComplement(Fblocks, + E, P, b, f, allKeys, keys_, augmentedHessian); + } + + /// Whiten the Jacobians computed by computeJacobians using noiseModel_ + void whitenJacobians(std::vector& F, Matrix& E, + Vector& b) const { + noiseModel_->WhitenSystem(E, b); + // TODO make WhitenInPlace work with any dense matrix type + BOOST_FOREACH(KeyMatrix2D& Fblock,F) + Fblock.second = noiseModel_->Whiten(Fblock.second); } /** * Return Jacobians as RegularImplicitSchurFactor with raw access */ - boost::shared_ptr > createRegularImplicitSchurFactor( - const Cameras& cameras, const Point3& point, double lambda = 0.0, - bool diagonalDamping = false) const { - std::vector F; + boost::shared_ptr > // + createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point, + double lambda = 0.0, bool diagonalDamping = false) const { Matrix E; Vector b; + std::vector F; computeJacobians(F, E, b, cameras, point); - noiseModel_->WhitenSystem(E, b); + whitenJacobians(F, E, b); Matrix3 P = PointCov(E, lambda, diagonalDamping); - // TODO make WhitenInPlace work with any dense matrix type - BOOST_FOREACH(KeyMatrix2D& Fblock,F) - Fblock.second = noiseModel_->Whiten(Fblock.second); - return boost::make_shared >(F, E, P, b); + return boost::make_shared >(F, E, P, + b); } /** @@ -511,12 +401,11 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector F; Matrix E; Vector b; + std::vector F; computeJacobians(F, E, b, cameras, point); const size_t M = b.size(); - std::cout << M << std::endl; Matrix3 P = PointCov(E, lambda, diagonalDamping); SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); return boost::make_shared >(F, E, P, b, n); diff --git a/gtsam/slam/SmartProjectionCameraFactor.h b/gtsam/slam/SmartProjectionCameraFactor.h index f10681ab8..3afd04188 100644 --- a/gtsam/slam/SmartProjectionCameraFactor.h +++ b/gtsam/slam/SmartProjectionCameraFactor.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 5b061f612..51f892a6d 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -35,14 +35,8 @@ namespace gtsam { * Structure for storing some state memory, used to speed up optimization * @addtogroup SLAM */ -class SmartProjectionFactorState { +struct SmartProjectionFactorState { -protected: - -public: - - SmartProjectionFactorState() { - } // Hessian representation (after Schur complement) bool calculatedHessian; Matrix H; @@ -68,38 +62,31 @@ private: protected: - // Some triangulation parameters - const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ + /// @name Caching triangulation + /// @{ + const TriangulationParameters parameters_; + mutable TriangulationResult result_; ///< result from triangulateSafe + const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses + /// @} + /// @name Parameters governing how triangulation result is treated + /// @{ const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} - const bool enableEPI_; ///< if set to true, will refine triangulation using LM + /// @name Caching linearization + /// @{ + /// shorthand for smart projection factor state variable + typedef boost::shared_ptr SmartFactorStatePtr; + SmartFactorStatePtr state_; ///< cached linearization const double linearizationThreshold_; ///< threshold to decide whether to re-linearize mutable std::vector cameraPosesLinearization_; ///< current linearization poses - - mutable Point3 point_; ///< Current estimate of the 3D point - - mutable bool degenerate_; - mutable bool cheiralityException_; - - // verbosity handling for Cheirality Exceptions - const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - - boost::shared_ptr state_; - - /// shorthand for smart projection factor state variable - typedef boost::shared_ptr SmartFactorStatePtr; - - double landmarkDistanceThreshold_; // if the landmark is triangulated at a - // distance larger than that the factor is considered degenerate - - double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the - // average reprojection error is smaller than this threshold after triangulation, - // and the factor is disregarded if the error is large + /// @} public: @@ -117,17 +104,18 @@ public: * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations */ - SmartProjectionFactor(const double rankTol, const double linThreshold, + SmartProjectionFactor(const double rankTolerance, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - 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) { + parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold, + dynamicOutlierRejectionThreshold), // + result_(TriangulationResult::Degenerate()), // + retriangulationThreshold_(1e-5), // + manageDegeneracy_(manageDegeneracy), // + throwCheirality_(false), verboseCheirality_(false), // + state_(state), linearizationThreshold_(linThreshold) { } /** Virtual destructor */ @@ -141,24 +129,23 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "SmartProjectionFactor, z = \n"; - std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; - std::cout << "degenerate_ = " << degenerate_ << std::endl; - std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + std::cout << s << "SmartProjectionFactor\n"; + std::cout << "triangulationParameters:\n" << parameters_ << std::endl; + std::cout << "result:\n" << result_ << std::endl; Base::print("", keyFormatter); } - /// Check if the new linearization point_ is the same as the one used for previous triangulation + /// Check if the new linearization point is the same as the one used for previous triangulation bool decideIfTriangulate(const Cameras& cameras) const { - // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate + // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate // Note that this is not yet "selecting linearization", that will come later, and we only check if the - // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_ + // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point size_t m = cameras.size(); bool retriangulate = false; - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + // if we do not have a previous linearization point or the new linearization point includes more poses if (cameraPosesTriangulation_.empty() || cameras.size() != cameraPosesTriangulation_.size()) retriangulate = true; @@ -181,19 +168,19 @@ public: cameraPosesTriangulation_.push_back(cameras[i].pose()); } - return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation + return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation } - /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization + /// This function checks if the new linearization point is 'close' to the previous one used for linearization bool decideIfLinearize(const Cameras& cameras) const { // "selective linearization" // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose // (we only care about the "rigidity" of the poses, not about their absolute pose) - if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize + if (linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize return true; - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + // if we do not have a previous linearization point or the new linearization point includes more poses if (cameraPosesLinearization_.empty() || (cameras.size() != cameraPosesLinearization_.size())) return true; @@ -211,100 +198,29 @@ public: Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); Pose3 localCameraPoseOld = firstCameraPoseOld.between( cameraPosesLinearization_[i]); - if (!localCameraPose.equals(localCameraPoseOld, - this->linearizationThreshold_)) + if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold_)) return true; // at least two "relative" poses are different, hence we re-linearize } - return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize + return false; // if we arrive to this point all poses are the same and we don't need re-linearize } /// triangulateSafe - size_t triangulateSafe(const Values& values) const { - return triangulateSafe(this->cameras(values)); - } - - /// triangulateSafe - size_t triangulateSafe(const Cameras& cameras) const { + TriangulationResult triangulateSafe(const Cameras& cameras) const { size_t m = cameras.size(); - if (m < 2) { // if we have a single pose the corresponding factor is uninformative - degenerate_ = true; - return m; - } + if (m < 2) // if we have a single pose the corresponding factor is uninformative + return TriangulationResult::Degenerate(); + bool retriangulate = decideIfTriangulate(cameras); - - if (retriangulate) { - // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(cameras, this->measured_, - rankTolerance_, enableEPI_); - degenerate_ = false; - cheiralityException_ = false; - - // Check landmark distance and reprojection errors to avoid outliers - double totalReprojError = 0.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_) { - degenerate_ = true; - break; - } - const Point2& zi = this->measured_.at(i); - try { - Point2 reprojectionError(camera.project(point_) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - cheiralityException_ = true; - } - i += 1; - } - // we discard smart factors that have large reprojection error - if (dynamicOutlierRejectionThreshold_ > 0 - && totalReprojError / m > dynamicOutlierRejectionThreshold_) - degenerate_ = true; - - } catch (TriangulationUnderconstrainedException&) { - // if TriangulationUnderconstrainedException can be - // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before - // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) - // in the second case we want to use a rotation-only smart factor - degenerate_ = true; - cheiralityException_ = false; - } catch (TriangulationCheiralityException&) { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers - // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint - cheiralityException_ = true; - } - } - return m; + if (retriangulate) + result_ = gtsam::triangulateSafe(cameras, this->measured_, parameters_); + return result_; } /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { - - bool isDebug = false; - size_t nrCameras = this->triangulateSafe(cameras); - - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) { - std::cout - << "createRegularImplicitSchurFactor: degenerate configuration" - << std::endl; - } - return false; - } else { - - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - } - return true; - } + triangulateSafe(cameras); // imperative, might reset result_ + return (manageDegeneracy_ || result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) @@ -324,12 +240,10 @@ public: exit(1); } - this->triangulateSafe(cameras); + triangulateSafe(cameras); - if (numKeys < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - // std::cout << "In linearize: exception" << std::endl; + if (!manageDegeneracy_ && !result_) { + // put in "empty" Hessian BOOST_FOREACH(Matrix& m, Gs) m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) @@ -338,23 +252,19 @@ public: Gs, gs, 0.0); } - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - } - + // decide whether to re-linearize bool doLinearize = this->decideIfLinearize(cameras); - if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize + if (linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize for (size_t i = 0; i < cameras.size(); i++) this->cameraPosesLinearization_[i] = cameras[i].pose(); if (!doLinearize) { // return the previous Hessian factor std::cout << "=============================" << std::endl; std::cout << "doLinearize " << doLinearize << std::endl; - std::cout << "this->linearizationThreshold_ " - << this->linearizationThreshold_ << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + std::cout << "linearizationThreshold_ " << linearizationThreshold_ + << std::endl; + std::cout << "valid: " << isValid() << std::endl; std::cout << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" << std::endl; @@ -370,6 +280,7 @@ public: { std::vector Fblocks; f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + Base::whitenJacobians(Fblocks,E,b); Base::FillDiagonalF(Fblocks, F); // expensive ! } @@ -404,7 +315,7 @@ public: } } // ================================================================== - if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables + if (linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables this->state_->Gs = Gs; this->state_->gs = gs; this->state_->f = f; @@ -417,7 +328,7 @@ public: boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createRegularImplicitSchurFactor(cameras, point_, lambda); + return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); else return boost::shared_ptr >(); } @@ -426,7 +337,7 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianQFactor(cameras, point_, lambda); + return Base::createJacobianQFactor(cameras, *result_, lambda); else return boost::make_shared >(this->keys_); } @@ -434,63 +345,27 @@ public: /// Create a factor, takes values boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { - Cameras cameras; - // TODO triangulate twice ?? - bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); - if (nonDegenerate) - return createJacobianQFactor(cameras, lambda); - else - return boost::make_shared >(this->keys_); + return createJacobianQFactor(this->cameras(values), lambda); } /// different (faster) way to compute Jacobian factor boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianSVDFactor(cameras, point_, lambda); + return Base::createJacobianSVDFactor(cameras, *result_, lambda); else return boost::make_shared >(this->keys_); } - /// Returns true if nonDegenerate - bool computeCamerasAndTriangulate(const Values& values, - Cameras& cameras) const { - Values valuesFactor; - - // Select only the cameras - BOOST_FOREACH(const Key key, this->keys_) - valuesFactor.insert(key, values.at(key)); - - cameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(cameras); - - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) - return false; - - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - - if (this->degenerate_) { - std::cout << "SmartProjectionFactor: this is not ready" << std::endl; - std::cout << "this->cheiralityException_ " << this->cheiralityException_ - << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; - } - return true; - } - /** * Triangulate and compute derivative of error with respect to point * @return whether triangulation worked */ bool triangulateAndComputeE(Matrix& E, const Values& values) const { - Cameras cameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - cameras.project2(point_, boost::none, E); + cameras.project2(*result_, boost::none, E); return nonDegenerate; } @@ -501,31 +376,18 @@ public: std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras) const { - if (this->degenerate_) { - std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; - std::cout << "point " << point_ << std::endl; - std::cout - << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used" - << std::endl; - if (Base::Dim > 6) { - std::cout - << "Management of degeneracy is not yet ready when one also optimizes for the calibration " - << std::endl; - } - + if (!result_) { + // TODO Luca clarify whether this works or not + result_ = TriangulationResult( + cameras[0].backprojectPointAtInfinity(this->measured_.at(0))); // TODO replace all this by Call to CameraSet int m = this->keys_.size(); E = zeros(2 * m, 2); b = zero(2 * m); double f = 0; for (size_t i = 0; i < this->measured_.size(); i++) { - if (i == 0) { // first pose - this->point_ = cameras[i].backprojectPointAtInfinity( - this->measured_.at(i)); - // 3D parametrization of point at infinity: [px py 1] - } Matrix Fi, Ei; - Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) + Vector bi = -(cameras[i].projectPointAtInfinity(*result_, Fi, Ei) - this->measured_.at(i)).vector(); f += bi.squaredNorm(); @@ -535,17 +397,17 @@ public: } return f; } else { - // nondegenerate: just return Base version - return Base::computeJacobians(Fblocks, E, b, cameras, point_); - } // end else + // valid result: just return Base version + return Base::computeJacobians(Fblocks, E, b, cameras, *result_); + } } /// Version that takes values, and creates the point bool triangulateAndComputeJacobians( std::vector& Fblocks, Matrix& E, Vector& b, const Values& values) const { - Cameras cameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); return nonDegenerate; @@ -555,19 +417,19 @@ public: bool triangulateAndComputeJacobiansSVD( std::vector& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { - typename Base::Cameras cameras; - double good = computeCamerasAndTriangulate(values, cameras); - if (good) - Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); - return true; + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + return nonDegenerate; } /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionErrorAfterTriangulation(const Values& values) const { - Cameras cameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - return Base::reprojectionError(cameras, point_); + return Base::reprojectionError(cameras, *result_); else return zero(cameras.size() * 2); } @@ -581,65 +443,61 @@ public: double totalReprojectionError(const Cameras& cameras, boost::optional externalPoint = boost::none) const { - size_t nrCameras; - if (externalPoint) { - nrCameras = this->keys_.size(); - point_ = *externalPoint; - degenerate_ = false; - cheiralityException_ = false; - } else { - nrCameras = this->triangulateSafe(cameras); - } + if (externalPoint) + result_ = TriangulationResult(*externalPoint); + else + result_ = triangulateSafe(cameras); - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - // if we don't want to manage the exceptions we discard the factor - // std::cout << "In error evaluation: exception" << std::endl; + // if we don't want to manage the exceptions we discard the factor + if (!manageDegeneracy_ && !result_) return 0.0; - } - if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors + if (isPointBehindCamera()) { // if we want to manage the exceptions with rotation-only factors std::cout << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" << std::endl; - this->degenerate_ = true; } - if (this->degenerate_) { + if (isDegenerate()) { // return 0.0; // TODO: this maybe should be zero? std::cout << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" << std::endl; // 3D parameterization of point at infinity - const Point2& zi = this->measured_.at(0); - this->point_ = cameras.front().backprojectPointAtInfinity(zi); - return Base::totalReprojectionErrorAtInfinity(cameras, this->point_); + const Point2& z0 = this->measured_.at(0); + result_ = TriangulationResult( + cameras.front().backprojectPointAtInfinity(z0)); + return Base::totalReprojectionErrorAtInfinity(cameras, *result_); } else { // Just use version in base class - return Base::totalReprojectionError(cameras, point_); + return Base::totalReprojectionError(cameras, *result_); } } /** return the landmark */ - boost::optional point() const { - return point_; + TriangulationResult point() const { + return result_; } /** COMPUTE the landmark */ - boost::optional point(const Values& values) const { - triangulateSafe(values); - return point_; + TriangulationResult point(const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateSafe(cameras); + } + + /// Is result valid? + inline bool isValid() const { + return result_; } /** return the degenerate state */ inline bool isDegenerate() const { - return (cheiralityException_ || degenerate_); + return result_.degenerate(); } /** return the cheirality status flag */ inline bool isPointBehindCamera() const { - return cheiralityException_; + return result_.behindCamera(); } /** return cheirality verbosity */ diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 8571a345d..3575a0286 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -255,6 +255,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0])); EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); + + // augmentedInformation (test just checks diagonals) + Matrix actualInfo = factor.augmentedInformation(); + EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12))); + + // information (test just checks diagonals) + Matrix actualInfo2 = factor.information(); + EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12))); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index ba7b7bf51..56ff47c3e 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -15,6 +15,7 @@ * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira + * @author Frank Dellaert * @date Sept 2013 */ @@ -133,9 +134,8 @@ TEST( SmartProjectionCameraFactor, noisy ) { using namespace vanilla; - // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2, 0.2); - Point2 level_uv = level_camera.project(landmark1) + pixelError; + // Project one landmark into two cameras and add noise on first + Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2); Point2 level_uv_right = level_camera_right.project(landmark1); Values values; @@ -147,7 +147,24 @@ TEST( SmartProjectionCameraFactor, noisy ) { factor1->add(level_uv, c1, unit2); factor1->add(level_uv_right, c2, unit2); + // Point is now uninitialized before a triangulation event + EXPECT(!factor1->point()); + + double expectedError = 58640; double actualError1 = factor1->error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1); + + // Check triangulated point + CHECK(factor1->point()); + EXPECT(assert_equal(Point3(13.7587, 1.43851, -1.14274),*factor1->point(), 1e-4)); + + // Check whitened errors + Vector expected(4); + expected << -7, 235, 58, -242; + SmartFactor::Cameras cameras1 = factor1->cameras(values); + Point3 point1 = *factor1->point(); + Vector actual = factor1->whitenedErrors(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; @@ -165,8 +182,7 @@ TEST( SmartProjectionCameraFactor, noisy ) { factor2->add(measurements, views, noises); double actualError2 = factor2->error(values); - - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1); } /* *************************************************************************/ @@ -174,57 +190,81 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { using namespace vanilla; + // Project three landmarks into three cameras vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + // Create and fill smartfactors + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); vector views; views.push_back(c1); views.push_back(c2); views.push_back(c3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); smartFactor1->add(measurements_cam1, views, unit2); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); smartFactor2->add(measurements_cam2, views, unit2); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); smartFactor3->add(measurements_cam3, views, unit2); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); - + // Create factor graph and add priors on two cameras NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); graph.push_back(PriorFactor(c1, cam1, noisePrior)); graph.push_back(PriorFactor(c2, cam2, noisePrior)); - Values values; - values.insert(c1, cam1); - values.insert(c2, cam2); - values.insert(c3, perturbCameraPose(cam3)); + // Create initial estimate + Values initial; + initial.insert(c1, cam1); + initial.insert(c2, cam2); + initial.insert(c3, perturbCameraPose(cam3)); if (isDebugTest) - values.at(c3).print("Smart: Pose3 before optimization: "); + initial.at(c3).print("Smart: Pose3 before optimization: "); + // Points are now uninitialized before a triangulation event + EXPECT(!smartFactor1->point()); + EXPECT(!smartFactor2->point()); + EXPECT(!smartFactor3->point()); + + EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1); + EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1); + EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1); + + // Error should trigger this and initialize the points, abort if not so + CHECK(smartFactor1->point()); + CHECK(smartFactor2->point()); + CHECK(smartFactor3->point()); + + EXPECT(assert_equal(Point3(2.57696, -0.182566, 1.04085),*smartFactor1->point(), 1e-4)); + EXPECT(assert_equal(Point3(2.80114, -0.702153, 1.06594),*smartFactor2->point(), 1e-4)); + EXPECT(assert_equal(Point3(1.82593, -0.289569, 2.13438),*smartFactor3->point(), 1e-4)); + + // Check whitened errors + Vector expected(6); + expected << 256, 29, -26, 29, -206, -202; + SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial); + Point3 point1 = *smartFactor1->point(); + Vector actual = smartFactor1->whitenedErrors(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); + + // Optimize LevenbergMarquardtParams params; - if (isDebugTest) + if (isDebugTest) { params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if (isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + } + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); - Values result; - gttic_(SmartProjectionCameraFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionCameraFactor); - tictoc_finishedIteration_(); + EXPECT(assert_equal(landmark1,*smartFactor1->point(), 1e-7)); + EXPECT(assert_equal(landmark2,*smartFactor2->point(), 1e-7)); + EXPECT(assert_equal(landmark3,*smartFactor3->point(), 1e-7)); - // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial); // VectorValues delta = GFG->optimize(); if (isDebugTest) @@ -243,8 +283,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { using namespace vanilla; + // Project three landmarks into three cameras vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); @@ -300,11 +340,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionCameraFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - gttoc_(SmartProjectionCameraFactor); - tictoc_finishedIteration_(); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); // VectorValues delta = GFG->optimize(); @@ -383,11 +420,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionCameraFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - gttoc_(SmartProjectionCameraFactor); - tictoc_finishedIteration_(); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); // VectorValues delta = GFG->optimize(); @@ -465,11 +499,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionCameraFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - gttoc_(SmartProjectionCameraFactor); - tictoc_finishedIteration_(); if (isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: "); @@ -544,11 +575,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionCameraFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - gttoc_(SmartProjectionCameraFactor); - tictoc_finishedIteration_(); if (isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: "); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index b235d8e2b..fae17b9a2 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -15,6 +15,7 @@ * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira + * @author Frank Dellaert * @date Sept 2013 */ @@ -38,7 +39,7 @@ static const bool manageDegeneracy = true; // Create a noise model for the pixel error static const double sigma = 0.1; -static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, sigma)); +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys using symbol_shorthand::X; @@ -289,7 +290,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { cameras.push_back(cam2); // Make sure triangulation works - LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras)); + CHECK(smartFactor1->triangulateSafe(cameras)); CHECK(!smartFactor1->isDegenerate()); CHECK(!smartFactor1->isPointBehindCamera()); boost::optional p = smartFactor1->point(); @@ -298,8 +299,11 @@ TEST( SmartProjectionPoseFactor, Factors ) { // After eliminating the point, A1 and A2 contain 2-rank information on cameras: Matrix16 A1, A2; - A1 << -1000, 0, 0, 0, 100, 0; - A2 << 1000, 0, 100, 0, -100, 0; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below { // createHessianFactor Matrix66 G11 = 0.5 * A1.transpose() * A1; @@ -314,10 +318,11 @@ TEST( SmartProjectionPoseFactor, Factors ) { double f = 0; RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); boost::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); - EXPECT(assert_equal(expected.information(), actual->information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); EXPECT(assert_equal(expected, *actual, 1e-8)); } @@ -336,36 +341,45 @@ TEST( SmartProjectionPoseFactor, Factors ) { F2(1, 0) = 100; F2(1, 2) = 10; F2(1, 4) = -10; - Matrix43 E; + Matrix E(4, 3); E.setZero(); - E(0, 0) = 100; - E(1, 1) = 100; - E(2, 0) = 100; - E(2, 2) = 10; - E(3, 1) = 100; - const vector > Fblocks = list_of > // + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + vector > Fblocks = list_of > // (make_pair(x1, F1))(make_pair(x2, F2)); - Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; + Vector b(4); b.setZero(); + // createJacobianQFactor + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8)); + EXPECT(assert_equal(expectedQ, *actualQ)); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + BOOST_FOREACH(SmartFactor::KeyMatrix2D& Fblock,Fblocks) + Fblock.second = model->Whiten(Fblock.second); + // createRegularImplicitSchurFactor - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b); boost::shared_ptr > actual = smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); EXPECT(assert_equal(expected, *actual)); - - // createJacobianQFactor - SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); - JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n); - - boost::shared_ptr > actualQ = - smartFactor1->createJacobianQFactor(cameras, 0.0); - CHECK(actual); - EXPECT(assert_equal(expectedQ.information(), actualQ->information(), 1e-8)); - EXPECT(assert_equal(expectedQ, *actualQ)); } { @@ -374,11 +388,12 @@ TEST( SmartProjectionPoseFactor, Factors ) { b.setZero(); double s = sin(M_PI_4); JacobianFactor expected(x1, s * A1, x2, s * A2, b); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); boost::shared_ptr actual = smartFactor1->createJacobianSVDFactor(cameras, 0.0); CHECK(actual); - EXPECT(assert_equal(expected.information(), actual->information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); EXPECT(assert_equal(expected, *actual)); } } @@ -976,7 +991,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) values.insert(x1, cam2); values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose_above * noise_pose,sharedK)); + values.insert(x3, Camera(pose_above * noise_pose, sharedK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -1070,9 +1085,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK)); - rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK)); - rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK)); + rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK)); + rotValues.insert(x2, Camera(poseDrift.compose(pose_right), sharedK)); + rotValues.insert(x3, Camera(poseDrift.compose(pose_above), sharedK)); boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); @@ -1086,9 +1101,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK)); - tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK)); - tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK)); + tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK)); + tranValues.insert(x2, Camera(poseDrift2.compose(pose_right), sharedK)); + tranValues.insert(x3, Camera(poseDrift2.compose(pose_above), sharedK)); boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); @@ -1130,9 +1145,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK2)); - rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK2)); - rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK2)); + rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK2)); + rotValues.insert(x2, Camera(poseDrift.compose(pose_right), sharedK2)); + rotValues.insert(x3, Camera(poseDrift.compose(pose_above), sharedK2)); boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); @@ -1148,9 +1163,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK2)); - tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK2)); - tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK2)); + tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK2)); + tranValues.insert(x2, Camera(poseDrift2.compose(pose_right), sharedK2)); + tranValues.insert(x3, Camera(poseDrift2.compose(pose_above), sharedK2)); boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); @@ -1230,7 +1245,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { values.insert(x1, cam2); values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK)); + values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -1336,7 +1351,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.insert(x1, cam2); values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK)); + values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: ");