improved and moved DLT function to Matrix.cpp
							parent
							
								
									dbe01bd0c8
								
							
						
					
					
						commit
						51b1650b08
					
				
							
								
								
									
										112
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										112
									
								
								.cproject
								
								
								
								
							|  | @ -390,6 +390,7 @@ | |||
| 					</target> | ||||
| 					<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testErrors.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -509,6 +510,7 @@ | |||
| 					</target> | ||||
| 					<target name="testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testBayesTree.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -516,6 +518,7 @@ | |||
| 					</target> | ||||
| 					<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testBinaryBayesNet.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -563,6 +566,7 @@ | |||
| 					</target> | ||||
| 					<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSymbolicBayesNet.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -570,6 +574,7 @@ | |||
| 					</target> | ||||
| 					<target name="testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSymbolicFactor.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -577,6 +582,7 @@ | |||
| 					</target> | ||||
| 					<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSymbolicFactorGraph.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -656,7 +662,6 @@ | |||
| 					</target> | ||||
| 					<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testGraph.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -752,7 +757,6 @@ | |||
| 					</target> | ||||
| 					<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testInference.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -760,7 +764,6 @@ | |||
| 					</target> | ||||
| 					<target name="testGaussianBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testGaussianBayesNet.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -768,7 +771,6 @@ | |||
| 					</target> | ||||
| 					<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testGaussianFactor.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -776,7 +778,6 @@ | |||
| 					</target> | ||||
| 					<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testJunctionTree.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -784,7 +785,6 @@ | |||
| 					</target> | ||||
| 					<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSymbolicBayesNet.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -792,7 +792,6 @@ | |||
| 					</target> | ||||
| 					<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSymbolicFactorGraph.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1008,7 +1007,6 @@ | |||
| 					</target> | ||||
| 					<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSimulated2DOriented.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1048,7 +1046,6 @@ | |||
| 					</target> | ||||
| 					<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSimulated2D.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1056,7 +1053,6 @@ | |||
| 					</target> | ||||
| 					<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSimulated3D.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1086,6 +1082,22 @@ | |||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="tests/testFundamental.run" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>tests/testFundamental.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="tests/testHomography2.run" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>tests/testHomography2.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="clean" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
|  | @ -1126,46 +1138,6 @@ | |||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>install</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>clean</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>check</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>all</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>dist</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
|  | @ -1262,6 +1234,46 @@ | |||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>install</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>clean</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>check</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>all</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>dist</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="check" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
|  |  | |||
|  | @ -1112,6 +1112,37 @@ void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V, bool sort) { | |||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) { | ||||
| 
 | ||||
| 	// Check size of A
 | ||||
| 	int m = A.size1(), n = A.size2(); | ||||
| 	if (m<n) throw invalid_argument("DLT: m<n, pad A with zero rows if needed."); | ||||
| 
 | ||||
| 	// Do SVD on A
 | ||||
| 	Matrix U, V; | ||||
| 	Vector S; | ||||
| 	static const bool sort = false; | ||||
| 	svd(A, U, S, V, sort); | ||||
| 
 | ||||
| 	// Find rank
 | ||||
| 	int rank = 0; | ||||
| 	for (int j = 0; j < n; j++) | ||||
| 		if (S(j) > rank_tol) | ||||
| 			rank++; | ||||
| 	// Find minimum singular value and corresponding column index
 | ||||
| 	int min_j = n - 1; | ||||
| 	double min_S = S(min_j); | ||||
| 	for (int j = 0; j < n - 1; j++) | ||||
| 		if (S(j) < min_S) { | ||||
| 			min_j = j; | ||||
| 			min_S = S(j); | ||||
| 		} | ||||
| 
 | ||||
| 	// Return rank, minimum singular value, and corresponding column of V
 | ||||
| 	return boost::tuple<int, double, Vector>(rank, min_S, column_(V, min_j)); | ||||
| } | ||||
| 
 | ||||
| #if 0 | ||||
| /* ************************************************************************* */ | ||||
| // TODO, would be faster with Cholesky
 | ||||
|  |  | |||
|  | @ -347,6 +347,9 @@ inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w( | |||
|  * @param sort boolean flag to sort singular values and V | ||||
|  * if m > n then U*S*V' = (m*n)*(n*n)*(n*n) (the m-n columns of U are of no use) | ||||
|  * if m < n then U*S*V' = (m*m)*(m*m)*(m*n) (the n-m columns of V are of no use) | ||||
|  * Careful! The dimensions above reflect V', not V, which is n*m if m<n. | ||||
|  * U is a basis in R^m, V is a basis in R^n | ||||
|  * You can just pass empty matrices U,V, and vector S, they will be re-allocated. | ||||
|  */  | ||||
| void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V, bool sort=true); | ||||
| 
 | ||||
|  | @ -357,9 +360,19 @@ void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V, bool sort=true); | |||
|  * @param V output argument: rotation matrix | ||||
|  * @param sort boolean flag to sort singular values and V | ||||
|  * if m > n then U*S*V' = (m*n)*(n*n)*(n*n) (the m-n columns of U are of no use) | ||||
|  * You can just pass empty matrix V and vector S, they will be re-allocated. | ||||
|  */ | ||||
| void svd(Matrix& A, Vector& S, Matrix& V, bool sort=true); | ||||
| 
 | ||||
| /**
 | ||||
|  * Direct linear transform algorithm that calls svd | ||||
|  * to find a vector v that minimizes the algebraic error A*v | ||||
|  * @param A of size m*n, where m>=n (pad with zero rows if not!) | ||||
|  * Returns rank of A, minimum error (singular value), | ||||
|  * and corresponding eigenvector (column of V, with A=U*S*V') | ||||
|  */ | ||||
| boost::tuple<int,double,Vector> DLT(const Matrix& A, double rank_tol=1e-9); | ||||
| 
 | ||||
| /** Use SVD to calculate inverse square root of a matrix */ | ||||
| Matrix inverse_square_root(const Matrix& A); | ||||
| 
 | ||||
|  |  | |||
|  | @ -12,27 +12,4 @@ using namespace tensors; | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| boost::tuple<int, double, Vector> DLT(const Matrix& A) { | ||||
| 
 | ||||
| 	// Do SVD on A
 | ||||
| 	Matrix U, V; | ||||
| 	Vector S; | ||||
| 	svd(A, U, S, V, false); | ||||
| 
 | ||||
| 	// Find minimum column
 | ||||
| 	int n = V.size2(), min_j = n - 1, rank = 0; | ||||
| 	for (int j = 0; j < n; j++) | ||||
| 		if (S(j) > 1e-9) | ||||
| 			rank++; | ||||
| 	double min_S = S(min_j); | ||||
| 	for (int j = 0; j < n - 1; j++) | ||||
| 		if (S(j) < min_S) { | ||||
| 			min_j = j; | ||||
| 			min_S = S(j); | ||||
| 		} | ||||
| 
 | ||||
| 	// Return minimum column
 | ||||
| 	return boost::tuple<int, double, Vector>(rank, min_S, column_(V, min_j)); | ||||
| } | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -94,10 +94,4 @@ namespace gtsam { | |||
| 		return R; | ||||
| 	} | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * Find Vector v that minimizes algebraic error A*v | ||||
| 	 * Returns rank of A, minimum error, and corresponding eigenvector | ||||
| 	 */ | ||||
| 	boost::tuple<int,double,Vector> DLT(const Matrix& A); | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue