Case change
							parent
							
								
									a95e5c7e05
								
							
						
					
					
						commit
						f2a5e5c683
					
				|  | @ -63,7 +63,7 @@ public: | |||
|       QF.push_back( | ||||
|           KeyMatrix(it.first, | ||||
|               (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); | ||||
|     JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); | ||||
|     JacobianFactor::fillTerms(QF, - Enull.transpose() * b, model); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -128,7 +128,7 @@ public: | |||
|    * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix | ||||
|    * Fast version - works on with sparsity | ||||
|    */ | ||||
|   static void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, | ||||
|   static void SparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, | ||||
|       const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, | ||||
|       /*output ->*/SymmetricBlockMatrix& augmentedHessian) { | ||||
|     // Schur complement trick
 | ||||
|  | @ -167,7 +167,7 @@ public: | |||
|    * 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( | ||||
|   static void UpdateSparseSchurComplement( | ||||
|       const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E, | ||||
|       const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, | ||||
|       const FastVector<Key>& allKeys, const FastVector<Key>& keys, | ||||
|  | @ -248,7 +248,7 @@ public: | |||
|     SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); | ||||
| 
 | ||||
|     // Do the Schur complement
 | ||||
|     sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); | ||||
|     SparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); | ||||
|     return augmentedHessian.matrix(); | ||||
|   } | ||||
| 
 | ||||
|  | @ -257,7 +257,7 @@ public: | |||
|     Matrix augmented = augmentedInformation(); | ||||
|     int m = this->keys_.size(); | ||||
|     size_t M = D * m; | ||||
|     return augmented.block(0,0,M,M); | ||||
|     return augmented.block(0, 0, M, M); | ||||
|   } | ||||
| 
 | ||||
|   /// Return the diagonal of the Hessian for this factor
 | ||||
|  |  | |||
|  | @ -272,7 +272,7 @@ public: | |||
|       EtE.diagonal() += lambda * EtE.diagonal(); | ||||
|     } else { | ||||
|       DenseIndex n = E.cols(); | ||||
|       EtE += lambda * Eigen::MatrixXd::Identity(n,n); | ||||
|       EtE += lambda * Eigen::MatrixXd::Identity(n, n); | ||||
|     } | ||||
| 
 | ||||
|     return (EtE).inverse(); | ||||
|  | @ -342,7 +342,8 @@ public: | |||
|     SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); | ||||
| 
 | ||||
|     // build augmented hessian
 | ||||
|     sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); | ||||
|     RegularImplicitSchurFactor<Dim>::SparseSchurComplement(Fblocks, E, P, b, | ||||
|         augmentedHessian); | ||||
|     augmentedHessian(m, m)(0, 0) = f; | ||||
| 
 | ||||
|     return boost::make_shared<RegularHessianFactor<Dim> >(keys_, | ||||
|  | @ -363,7 +364,7 @@ public: | |||
|     std::vector<KeyMatrix2D> Fblocks; | ||||
|     double f = computeJacobians(Fblocks, E, b, cameras, point); | ||||
|     Matrix3 P = PointCov(E, lambda, diagonalDamping); | ||||
|     RegularImplicitSchurFactor<Dim, ZDim>::updateSparseSchurComplement(Fblocks, | ||||
|     RegularImplicitSchurFactor<Dim, ZDim>::UpdateSparseSchurComplement(Fblocks, | ||||
|         E, P, b, f, allKeys, keys_, augmentedHessian); | ||||
|   } | ||||
| 
 | ||||
|  | @ -420,7 +421,8 @@ public: | |||
|     const size_t M = ZDim * m; | ||||
|     Matrix E0(M, M - 3); | ||||
|     computeJacobiansSVD(F, E0, b, cameras, point); | ||||
|     SharedIsotropic n = noiseModel::Isotropic::Sigma(M-3, noiseModel_->sigma()); | ||||
|     SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3, | ||||
|         noiseModel_->sigma()); | ||||
|     return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(F, E0, b, n); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue