Replaced the householder transform with the weighted system
Removed constrained components from makefile, they will disappear shortlyrelease/4.3a0
Alex Cunningham
parent
03985d16f6
commit
2c37c94b5d
44
.cproject
44
.cproject
|
|
@ -300,6 +300,7 @@
|
|||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -307,6 +308,7 @@
|
|||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -314,7 +316,7 @@
|
|||
</target>
|
||||
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments>-k</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -322,6 +324,7 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -337,7 +340,6 @@
|
|||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -345,6 +347,7 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -352,7 +355,6 @@
|
|||
</target>
|
||||
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConditionalGaussian.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -360,6 +362,7 @@
|
|||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -375,6 +378,7 @@
|
|||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -382,7 +386,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -390,20 +393,15 @@
|
|||
</target>
|
||||
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -411,7 +409,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -419,22 +416,14 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVectorConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -442,7 +431,6 @@
|
|||
</target>
|
||||
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -450,6 +438,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -457,6 +446,7 @@
|
|||
</target>
|
||||
<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -464,6 +454,7 @@
|
|||
</target>
|
||||
<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -471,6 +462,7 @@
|
|||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -478,7 +470,6 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -486,6 +477,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -493,7 +485,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -501,6 +492,7 @@
|
|||
</target>
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -508,6 +500,7 @@
|
|||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -515,7 +508,6 @@
|
|||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -523,7 +515,6 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -531,7 +522,6 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -198,10 +198,7 @@ Matrix LinearFactor::matrix_augmented(const Ordering& ordering) const {
|
|||
B_mat(i,0) = b_(i);
|
||||
matrices.push_back(&B_mat);
|
||||
|
||||
// divide in sigma so error is indeed 0.5*|Ax-b|
|
||||
Vector t = ediv(ones(sigmas_.size()),sigmas_);
|
||||
Matrix A = vector_scale(collect(matrices), t);
|
||||
return A;
|
||||
return collect(matrices);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -307,8 +304,8 @@ LinearFactor::eliminate(const string& key)
|
|||
|
||||
// extract A, b from the combined linear factor (ensure that x is leading)
|
||||
// use an augmented system [A b] to prevent copying
|
||||
Matrix Rd = matrix_augmented(ordering);
|
||||
size_t m = Rd.size1(); size_t n = Rd.size2()-1;
|
||||
Matrix Ab = matrix_augmented(ordering);
|
||||
size_t m = Ab.size1(); size_t n = Ab.size2()-1;
|
||||
|
||||
// get dimensions of the eliminated variable
|
||||
size_t n1 = getDim(key);
|
||||
|
|
@ -319,25 +316,18 @@ LinearFactor::eliminate(const string& key)
|
|||
throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
|
||||
|
||||
// Do in-place QR to get R, d of the augmented system
|
||||
if (verbose) ::print(Rd,"Rd before");
|
||||
householder(Rd, maxRank);
|
||||
if (verbose) ::print(Ab,"Rd before");
|
||||
Matrix Rd; Vector sigmas;
|
||||
boost::tie(Rd, sigmas) = weighted_eliminate(Ab, sigmas_);
|
||||
if (verbose) ::print(Rd,"Rd after");
|
||||
|
||||
// R as calculated by householder has inverse sigma on diagonal
|
||||
// Use them to normalize R to unit-upper-triangular matrix
|
||||
Vector sigmas(m); // standard deviations
|
||||
if (verbose) cout << n1 << " " << n << " " << m << endl;
|
||||
for (int i=0; i<maxRank; ++i) {
|
||||
double Rii = Rd(i,i);
|
||||
if (fabs(Rii)<1e-8) { maxRank=i; break;} // detect if rank < maxRank
|
||||
sigmas(i) = 1.0/Rii; // calculate sigma
|
||||
for (int j=0; j<=n; ++j) Rd(i,j) = Rd(i,j)*sigmas(i); // normalize
|
||||
if (sigmas(i)<0) sigmas(i)=-sigmas(i); // make sure sigma positive
|
||||
}
|
||||
//NOTE: this actually normalizes the entire R matrix automatically,
|
||||
// and produces the precisions
|
||||
// This could probably be made more efficient, but is accurate
|
||||
|
||||
// extract RHS
|
||||
Vector d(m);
|
||||
for (int i=0; i<m; ++i)
|
||||
Vector d(maxRank);
|
||||
for (int i=0; i<maxRank; ++i)
|
||||
d(i) = Rd(i,n);
|
||||
|
||||
// create base conditional Gaussian
|
||||
|
|
|
|||
|
|
@ -125,8 +125,8 @@ public:
|
|||
/** get a copy of b */
|
||||
const Vector& get_b() const { return b_; }
|
||||
|
||||
/** get a copy of precisions */
|
||||
const Vector& get_precisions() const { return sigmas_; }
|
||||
/** get a copy of sigmas */
|
||||
const Vector& get_sigmas() const { return sigmas_; }
|
||||
|
||||
/**
|
||||
* get a copy of the A matrix from a specific node
|
||||
|
|
@ -193,7 +193,7 @@ public:
|
|||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* The returned system is an augmented matrix: [A b]
|
||||
* As above, the standard deviations are baked into A and b
|
||||
* The standard deviations are NOT baked into A and b
|
||||
* @param ordering of variables needed for matrix column order
|
||||
*/
|
||||
Matrix matrix_augmented(const Ordering& ordering) const;
|
||||
|
|
|
|||
|
|
@ -58,26 +58,21 @@ testMatrix_LDADD = libgtsam.la
|
|||
# nodes
|
||||
sources += VectorConfig.cpp Ordering.cpp
|
||||
sources += SymbolicFactor.cpp LinearFactor.cpp NonlinearFactor.cpp
|
||||
sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp
|
||||
sources += ConditionalGaussian.cpp
|
||||
check_PROGRAMS += testVectorConfig testSymbolicFactor testLinearFactor
|
||||
check_PROGRAMS += testConditionalGaussian testNonlinearFactor
|
||||
check_PROGRAMS += testLinearConstraint testConstrainedConditionalGaussian
|
||||
example = smallExample.cpp
|
||||
testVectorConfig_SOURCES = testVectorConfig.cpp
|
||||
testSymbolicFactor_SOURCES = $(example) testSymbolicFactor.cpp
|
||||
testLinearFactor_SOURCES = $(example) testLinearFactor.cpp
|
||||
testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
|
||||
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
|
||||
testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp
|
||||
testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp
|
||||
|
||||
testVectorConfig_LDADD = libgtsam.la
|
||||
testSymbolicFactor_LDADD = libgtsam.la
|
||||
testLinearFactor_LDADD = libgtsam.la
|
||||
testConditionalGaussian_LDADD = libgtsam.la
|
||||
testNonlinearFactor_LDADD = libgtsam.la
|
||||
testLinearConstraint_LDADD = libgtsam.la
|
||||
testConstrainedConditionalGaussian_LDADD = libgtsam.la
|
||||
|
||||
# not the correct way, I'm sure: Kai ?
|
||||
timeLinearFactor: timeLinearFactor.cpp
|
||||
|
|
@ -87,12 +82,10 @@ timeLinearFactor: LDFLAGS += -L.libs -lgtsam
|
|||
# graphs
|
||||
sources += SymbolicFactorGraph.cpp LinearFactorGraph.cpp
|
||||
sources += SymbolicBayesNet.cpp GaussianBayesNet.cpp
|
||||
sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp
|
||||
check_PROGRAMS += testFactorgraph testSymbolicFactorGraph
|
||||
check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph
|
||||
check_PROGRAMS += testGaussianBayesNet testNonlinearOptimizer
|
||||
check_PROGRAMS += testSymbolicBayesNet testBayesTree
|
||||
check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph
|
||||
testFactorgraph_SOURCES = testFactorgraph.cpp
|
||||
testSymbolicFactorGraph_SOURCES = $(example) testSymbolicFactorGraph.cpp
|
||||
testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp
|
||||
|
|
@ -101,8 +94,6 @@ testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
|
|||
testSymbolicBayesNet_SOURCES = $(example) testSymbolicBayesNet.cpp
|
||||
testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp
|
||||
testBayesTree_SOURCES = $(example) testBayesTree.cpp
|
||||
testConstrainedNonlinearFactorGraph_SOURCES = $(example) testConstrainedNonlinearFactorGraph.cpp
|
||||
testConstrainedLinearFactorGraph_SOURCES = $(example) testConstrainedLinearFactorGraph.cpp
|
||||
|
||||
testFactorgraph_LDADD = libgtsam.la
|
||||
testSymbolicFactorGraph_LDADD = libgtsam.la
|
||||
|
|
@ -112,8 +103,6 @@ testNonlinearOptimizer_LDADD = libgtsam.la
|
|||
testSymbolicBayesNet_LDADD = libgtsam.la
|
||||
testGaussianBayesNet_LDADD = libgtsam.la
|
||||
testBayesTree_LDADD = libgtsam.la
|
||||
testConstrainedNonlinearFactorGraph_LDADD = libgtsam.la
|
||||
testConstrainedLinearFactorGraph_LDADD = libgtsam.la
|
||||
|
||||
# not the correct way, I'm sure: Kai ?
|
||||
timeLinearFactorGraph: timeLinearFactorGraph.cpp
|
||||
|
|
|
|||
|
|
@ -148,6 +148,18 @@ Vector Vector_(const Matrix& A)
|
|||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector column(const Matrix& A, size_t j) {
|
||||
if (j>=A.size2())
|
||||
throw invalid_argument("Column index out of bounds!");
|
||||
|
||||
size_t m = A.size1();
|
||||
Vector a(m);
|
||||
for (int i=0; i<m; ++i)
|
||||
a(i) = A(i,j);
|
||||
return a;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void print(const Matrix& A, const string &s) {
|
||||
size_t m = A.size1(), n = A.size2();
|
||||
|
|
@ -273,27 +285,43 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void whouse_subs(Matrix& A, unsigned int row, const Vector& a, const Vector& pseudo) {
|
||||
// cout << "In matrix::whouse_subs()" << endl;
|
||||
std::pair<Matrix, Vector> weighted_eliminate(Matrix& A, const Vector& sigmas) {
|
||||
// get sizes
|
||||
size_t m = A.size1(); size_t n = A.size2();
|
||||
// print(A,"Initial A");
|
||||
// cout << "Row: " << row << endl;
|
||||
// print(a,"a");
|
||||
// print(pseudo,"pseudo");
|
||||
size_t m = A.size1();
|
||||
size_t n = A.size2();
|
||||
size_t maxRank = min(m,n);
|
||||
|
||||
// calculate product = pseudo*A
|
||||
Vector product = zero(n);
|
||||
for (int j=0; j<n; ++j) // for each column in A
|
||||
for (int i=row;i<m;++i) {// only the relevant section of A
|
||||
product(j) += A(i,j)*pseudo(i-row);
|
||||
// create an R matrix to store the solution
|
||||
Matrix R = eye(maxRank, n);
|
||||
|
||||
// create a sigma vector
|
||||
Vector newSigmas(maxRank);
|
||||
|
||||
// loop over the columns
|
||||
for (int j=0; j<maxRank; ++j) {
|
||||
// extract the first column of A
|
||||
Vector a = column(A, j);
|
||||
|
||||
// find weighted pseudoinverse
|
||||
Vector pseudo; double precision;
|
||||
boost::tie(pseudo, precision) = weightedPseudoinverse(a, sigmas);
|
||||
|
||||
// create solution and copy into R
|
||||
for (int j2=j; j2<n; ++j2) {
|
||||
R(j,j2) = inner_prod(pseudo, column(A, j2));
|
||||
}
|
||||
|
||||
// calculate A -= a*product
|
||||
for (int i=row+1;i<m;++i) // limit only to rows of A being updated
|
||||
for (int j=0;j<n;++j) { // update all columns
|
||||
A(i,j) -= product(j)*a(i-row);
|
||||
}
|
||||
// update A
|
||||
for (int i=0;i<m;++i) // update all rows
|
||||
for (int j2=j+1;j2<n;++j2) { // limit to only columns in separator
|
||||
A(i,j2) -= R(j,j2)*a(i);
|
||||
}
|
||||
|
||||
// save precision information
|
||||
newSigmas[j] = sqrt(1./precision);
|
||||
}
|
||||
|
||||
return make_pair(R, newSigmas);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
23
cpp/Matrix.h
23
cpp/Matrix.h
|
|
@ -110,6 +110,14 @@ void print(const Matrix& A, const std::string& s = "");
|
|||
*/
|
||||
Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2);
|
||||
|
||||
/**
|
||||
* extracts a column from a matrix
|
||||
* @param matrix to extract column from
|
||||
* @param index of the column
|
||||
* @return the column in vector form
|
||||
*/
|
||||
Vector column(const Matrix& A, size_t j);
|
||||
|
||||
// left multiply with scalar
|
||||
//inline Matrix operator*(double s, const Matrix& A) { return A*s;}
|
||||
|
||||
|
|
@ -137,15 +145,14 @@ std::pair<Matrix,Matrix> qr(const Matrix& A);
|
|||
*/
|
||||
void householder_update(Matrix &A, int j, double beta, const Vector& vjm);
|
||||
|
||||
/*
|
||||
* Imperative version of weighted Householder substitution
|
||||
* @param A is the matrix to reduce
|
||||
* @param row is the row to start on (rows above aren't affected)
|
||||
* @param pseudo is the pseudoinverse of the first column of A
|
||||
* @param a is the first column of A
|
||||
* A is updated into non-normalized R of A that has been updated
|
||||
/**
|
||||
* Imperative algorithm for in-place full elimination with
|
||||
* weights and constraint handling
|
||||
* @param Ab is an augmented system to eliminate
|
||||
* @param sigmas is a vector of the measurement standard deviation
|
||||
* @return a pair of R (normalized) and new sigmas
|
||||
*/
|
||||
void whouse_subs(Matrix& A, unsigned int row, const Vector& a, const Vector& pseudo);
|
||||
std::pair<Matrix, Vector> weighted_eliminate(Matrix& Ab, const Vector& sigmas);
|
||||
|
||||
/**
|
||||
* Householder tranformation, Householder vectors below diagonal
|
||||
|
|
|
|||
|
|
@ -182,32 +182,19 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector whouse_solve(const Vector& v, const Vector& precisions) {
|
||||
if (v.size() != precisions.size())
|
||||
pair<Vector, double> weightedPseudoinverse(const Vector& v, const Vector& sigmas) {
|
||||
if (v.size() != sigmas.size())
|
||||
throw invalid_argument("V and precisions have different sizes!");
|
||||
double normV = 0;
|
||||
for(int i = 0; i<v.size(); i++)
|
||||
Vector precisions(sigmas.size());
|
||||
for(int i = 0; i<v.size(); i++) {
|
||||
precisions[i] = 1./(sigmas[i]*sigmas[i]);
|
||||
normV += v[i]*v[i]*precisions[i];
|
||||
}
|
||||
Vector sol(v.size());
|
||||
for(int i = 0; i<v.size(); i++)
|
||||
sol[i] = precisions[i]*v[i];
|
||||
return sol/normV;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void whouse_subs(Vector& b, size_t row, const Vector& a, const Vector& pseudo) {
|
||||
// get sizes
|
||||
size_t m = b.size();
|
||||
|
||||
// calculate product = pseudo*b
|
||||
double product = 0;
|
||||
for (int i=row;i<m;++i) {// only the relevant section of b
|
||||
product += b(i)*pseudo(i-row);
|
||||
}
|
||||
|
||||
// calculate b -= a*product
|
||||
for (int i=row+1;i<m;++i)
|
||||
b(i) -= a(i-row)*product;
|
||||
return make_pair(sol/normV, normV);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
15
cpp/Vector.h
15
cpp/Vector.h
|
|
@ -110,19 +110,10 @@ std::pair<double,Vector> house(Vector &x);
|
|||
* Weighted Householder solution vector,
|
||||
* a.k.a., the pseudoinverse of the column
|
||||
* @param v is the first column of the matrix to solve
|
||||
* @param precisions is a vector of precisions ( sigma^(-2) )
|
||||
* @return the pseudoinverse of v
|
||||
* @param simgas is a vector of standard deviations
|
||||
* @return a pair of the pseudoinverse of v and the precision
|
||||
*/
|
||||
Vector whouse_solve(const Vector& v, const Vector& precisions);
|
||||
|
||||
/**
|
||||
* Weighted Householder solution substitution into a vector
|
||||
* @param b is vector to update IN PLACE
|
||||
* @param row is the row being updated (the specified row is not touched)
|
||||
* @param a is the first column of A
|
||||
* @param pseudo is the pseudoinverse of a
|
||||
*/
|
||||
void whouse_subs(Vector& b, size_t row, const Vector& a, const Vector& pseudo);
|
||||
std::pair<Vector, double> weightedPseudoinverse(const Vector& v, const Vector& sigmas);
|
||||
|
||||
/**
|
||||
* concatenate Vectors
|
||||
|
|
|
|||
|
|
@ -14,7 +14,6 @@ using namespace std;
|
|||
#include "Ordering.h"
|
||||
#include "Matrix.h"
|
||||
#include "NonlinearFactor.h"
|
||||
#include "ConstrainedLinearFactorGraph.h"
|
||||
#include "smallExample.h"
|
||||
#include "Point2Prior.h"
|
||||
#include "Simulated2DOdometry.h"
|
||||
|
|
@ -283,7 +282,45 @@ LinearFactorGraph createSmoother(int T) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConstrainedLinearFactorGraph createSingleConstraintGraph() {
|
||||
LinearFactorGraph createSimpleConstraintGraph() {
|
||||
// create unary factor
|
||||
// prior on "x", mean = [1,-1], sigma=0.1
|
||||
double sigma = 0.1;
|
||||
Matrix Ax = eye(2);
|
||||
Vector b1(2);
|
||||
b1(0) = 1.0;
|
||||
b1(1) = -1.0;
|
||||
LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1, sigma));
|
||||
|
||||
// create binary constraint factor
|
||||
// between "x" and "y", that is going to be the only factor on "y"
|
||||
// |1 0||x_1| + |-1 0||y_1| = |0|
|
||||
// |0 1||x_2| | 0 -1||y_2| |0|
|
||||
Matrix Ax1 = eye(2);
|
||||
Matrix Ay1 = eye(2) * -1;
|
||||
Vector b2 = Vector_(2, 0.0, 0.0);
|
||||
LinearFactor::shared_ptr f2(
|
||||
new LinearFactor("x", Ax1, "y", Ay1, b2, 0.0));
|
||||
|
||||
// construct the graph
|
||||
LinearFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back(f2);
|
||||
|
||||
return fg;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorConfig createSimpleConstraintConfig() {
|
||||
VectorConfig config;
|
||||
Vector v = Vector_(2, 1.0, -1.0);
|
||||
config.insert("x", v);
|
||||
config.insert("y", v);
|
||||
return config;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearFactorGraph createSingleConstraintGraph() {
|
||||
// create unary factor
|
||||
// prior on "x", mean = [1,-1], sigma=0.1
|
||||
double sigma = 0.1;
|
||||
|
|
@ -302,19 +339,27 @@ ConstrainedLinearFactorGraph createSingleConstraintGraph() {
|
|||
Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
|
||||
Matrix Ay1 = eye(2) * 10;
|
||||
Vector b2 = Vector_(2, 1.0, 2.0);
|
||||
LinearConstraint::shared_ptr f2(
|
||||
new LinearConstraint("x", Ax1, "y", Ay1, b2));
|
||||
LinearFactor::shared_ptr f2(
|
||||
new LinearFactor("x", Ax1, "y", Ay1, b2, 0.0));
|
||||
|
||||
// construct the graph
|
||||
ConstrainedLinearFactorGraph fg;
|
||||
LinearFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back_constraint(f2);
|
||||
fg.push_back(f2);
|
||||
|
||||
return fg;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConstrainedLinearFactorGraph createMultiConstraintGraph() {
|
||||
VectorConfig createSingleConstraintConfig() {
|
||||
VectorConfig config;
|
||||
config.insert("x", Vector_(2, 1.0, -1.0));
|
||||
config.insert("y", Vector_(2, 0.2, 0.1));
|
||||
return config;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearFactorGraph createMultiConstraintGraph() {
|
||||
// unary factor 1
|
||||
double sigma = 0.1;
|
||||
Matrix A = eye(2);
|
||||
|
|
@ -332,7 +377,7 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
|
|||
|
||||
Vector b1(2);
|
||||
b1(0) = 1.0; b1(1) = 2.0;
|
||||
LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
|
||||
LinearFactor::shared_ptr lc1(new LinearFactor("x", A11, "y", A12, b1, 0.0));
|
||||
|
||||
// constraint 2
|
||||
Matrix A21(2,2);
|
||||
|
|
@ -345,25 +390,34 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
|
|||
|
||||
Vector b2(2);
|
||||
b2(0) = 3.0; b2(1) = 4.0;
|
||||
LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
|
||||
LinearFactor::shared_ptr lc2(new LinearFactor("x", A21, "z", A22, b2, 0.0));
|
||||
|
||||
// construct the graph
|
||||
ConstrainedLinearFactorGraph fg;
|
||||
LinearFactorGraph fg;
|
||||
fg.push_back(lf1);
|
||||
fg.push_back_constraint(lc1);
|
||||
fg.push_back_constraint(lc2);
|
||||
fg.push_back(lc1);
|
||||
fg.push_back(lc2);
|
||||
|
||||
return fg;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph()
|
||||
VectorConfig createMultiConstraintConfig() {
|
||||
VectorConfig config;
|
||||
config.insert("x", Vector_(2, -2.0, 2.0));
|
||||
config.insert("y", Vector_(2, -0.1, 0.4));
|
||||
config.insert("z", Vector_(2, -4.0, 5.0));
|
||||
return config;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//LinearFactorGraph createConstrainedLinearFactorGraph()
|
||||
//{
|
||||
// ConstrainedLinearFactorGraph graph;
|
||||
// LinearFactorGraph graph;
|
||||
//
|
||||
// // add an equality factor
|
||||
// Vector v1(2); v1(0)=1.;v1(1)=2.;
|
||||
// LinearConstraint::shared_ptr f1(new LinearConstraint(v1, "x0"));
|
||||
// LinearFactor::shared_ptr f1(new LinearFactor(v1, "x0"));
|
||||
// graph.push_back_eq(f1);
|
||||
//
|
||||
// // add a normal linear factor
|
||||
|
|
@ -386,7 +440,7 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
|
|||
// VectorConfig c = createConstrainedConfig();
|
||||
//
|
||||
// // equality constraint for initial pose
|
||||
// LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0"));
|
||||
// LinearFactor::shared_ptr f1(new LinearFactor(c["x0"], "x0"));
|
||||
// graph.push_back_eq(f1);
|
||||
//
|
||||
// // odometry between x0 and x1
|
||||
|
|
|
|||
|
|
@ -15,8 +15,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class ConstrainedLinearFactorGraph;
|
||||
|
||||
typedef NonlinearFactorGraph<VectorConfig> ExampleNonlinearFactorGraph;
|
||||
|
||||
/**
|
||||
|
|
@ -75,17 +73,26 @@ namespace gtsam {
|
|||
// Constrained Examples
|
||||
/* ******************************************************* */
|
||||
|
||||
/**
|
||||
* Creates a simple constrained graph with one linear factor and
|
||||
* one binary equality constraint that sets x = y
|
||||
*/
|
||||
LinearFactorGraph createSimpleConstraintGraph();
|
||||
VectorConfig createSimpleConstraintConfig();
|
||||
|
||||
/**
|
||||
* Creates a simple constrained graph with one linear factor and
|
||||
* one binary constraint.
|
||||
*/
|
||||
ConstrainedLinearFactorGraph createSingleConstraintGraph();
|
||||
LinearFactorGraph createSingleConstraintGraph();
|
||||
VectorConfig createSingleConstraintConfig();
|
||||
|
||||
/**
|
||||
* Creates a constrained graph with a linear factor and two
|
||||
* binary constraints that share a node
|
||||
*/
|
||||
ConstrainedLinearFactorGraph createMultiConstraintGraph();
|
||||
LinearFactorGraph createMultiConstraintGraph();
|
||||
VectorConfig createMultiConstraintConfig();
|
||||
|
||||
/**
|
||||
* These are the old examples from the EqualityFactor/DeltaFunction
|
||||
|
|
@ -110,7 +117,7 @@ namespace gtsam {
|
|||
/**
|
||||
* Create small example constrained factor graph
|
||||
*/
|
||||
//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph();
|
||||
//LinearFactorGraph createConstrainedLinearFactorGraph();
|
||||
|
||||
/**
|
||||
* Create small example constrained nonlinear factor graph
|
||||
|
|
|
|||
|
|
@ -523,8 +523,8 @@ TEST( LinearFactor, matrix_aug )
|
|||
Ab = lf->matrix_augmented(ord);
|
||||
|
||||
Matrix Ab1 = Matrix_(2,5,
|
||||
-10.0, 0.0, 10.0, 0.0, 2.0,
|
||||
000.0,-10.0, 0.0, 10.0, -1.0 );
|
||||
-1.0, 0.0, 1.0, 0.0, 0.2,
|
||||
00.0,- 1.0, 0.0, 1.0, -0.1 );
|
||||
|
||||
EQUALITY(Ab,Ab1);
|
||||
}
|
||||
|
|
@ -635,6 +635,70 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
|
|||
|
||||
CHECK(assert_equal(expectedLF,actualLF,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST ( LinearFactor, constraint_eliminate1 )
|
||||
{
|
||||
// // construct a linear constraint
|
||||
// Vector v(2); v(0)=1.2; v(1)=3.4;
|
||||
// string key = "x0";
|
||||
// LinearFactor lc(key, eye(2), v, 0.0);
|
||||
//
|
||||
// // eliminate it
|
||||
// ConditionalGaussian::shared_ptr actualCG;
|
||||
// LinearFactor::shared_ptr actualLF;
|
||||
// boost::tie(actualCG,actualLF) = lc.eliminate("x0");
|
||||
//
|
||||
// // verify linear factor
|
||||
// CHECK(actualLF->size() == 0);
|
||||
//
|
||||
// // verify conditional Gaussian
|
||||
// Vector sigmas = Vector_(2, 0.0, 0.0);
|
||||
// ConditionalGaussian expCG("x0", v, eye(2), sigmas);
|
||||
// CHECK(assert_equal(expCG, *actualCG)); // FAILS - gets NaN values
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST ( LinearFactor, constraint_eliminate2 )
|
||||
{
|
||||
// // Construct a linear constraint
|
||||
// // RHS
|
||||
// Vector b(2); b(0)=3.0; b(1)=4.0;
|
||||
//
|
||||
// // A1 - invertible
|
||||
// Matrix A1(2,2);
|
||||
// A1(0,0) = 1.0 ; A1(0,1) = 2.0;
|
||||
// A1(1,0) = 2.0 ; A1(1,1) = 1.0;
|
||||
//
|
||||
// // A2 - not invertible - solve will throw an exception
|
||||
// Matrix A2(2,2);
|
||||
// A2(0,0) = 1.0 ; A2(0,1) = 2.0;
|
||||
// A2(1,0) = 2.0 ; A2(1,1) = 4.0;
|
||||
//
|
||||
// LinearFactor lc("x", A1, "y", A2, b, 0.0);
|
||||
//
|
||||
// Vector y = Vector_(2, 1.0, 2.0);
|
||||
//
|
||||
// VectorConfig fg1;
|
||||
// fg1.insert("y", y);
|
||||
//
|
||||
// Vector expected = Vector_(2, -3.3333, 0.6667);
|
||||
//
|
||||
// // eliminate x for basic check
|
||||
// ConditionalGaussian::shared_ptr actual = lc.eliminate("x");
|
||||
// CHECK(assert_equal(expected, actual->solve(fg1), 1e-4));
|
||||
//
|
||||
// // eliminate y to test thrown error
|
||||
// VectorConfig fg2;
|
||||
// fg2.insert("x", expected);
|
||||
// actual = lc.eliminate("y");
|
||||
// try {
|
||||
// Vector output = actual->solve(fg2);
|
||||
// CHECK(false);
|
||||
// } catch (...) {
|
||||
// CHECK(true);
|
||||
// }
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -585,6 +585,57 @@ TEST( LinearFactorGraph, variables )
|
|||
CHECK(expected==actual);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Tests ported from ConstrainedLinearFactorGraph
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( LinearFactorGraph, constrained_simple )
|
||||
//{
|
||||
// // get a graph with a constraint in it
|
||||
// LinearFactorGraph fg = createSimpleConstraintGraph();
|
||||
//
|
||||
// // eliminate and solve
|
||||
// Ordering ord;
|
||||
// ord += "x", "y";
|
||||
// VectorConfig actual = fg.optimize(ord);
|
||||
//
|
||||
// // verify
|
||||
// VectorConfig expected = createSimpleConstraintConfig();
|
||||
// CHECK(assert_equal(actual, expected));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( LinearFactorGraph, constrained_single )
|
||||
//{
|
||||
// // get a graph with a constraint in it
|
||||
// LinearFactorGraph fg = createSingleConstraintGraph();
|
||||
//
|
||||
// // eliminate and solve
|
||||
// Ordering ord;
|
||||
// ord += "x", "y";
|
||||
// VectorConfig actual = fg.optimize(ord);
|
||||
//
|
||||
// // verify
|
||||
// VectorConfig expected = createSingleConstraintConfig();
|
||||
// CHECK(assert_equal(actual, expected));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( LinearFactorGraph, constrained_multi )
|
||||
//{
|
||||
// // get a graph with a constraint in it
|
||||
// LinearFactorGraph fg = createMultiConstraintGraph();
|
||||
//
|
||||
// // eliminate and solve
|
||||
// Ordering ord;
|
||||
// ord += "x", "y", "z";
|
||||
// VectorConfig actual = fg.optimize(ord);
|
||||
//
|
||||
// // verify
|
||||
// VectorConfig expected = createMultiConstraintConfig();
|
||||
// CHECK(assert_equal(actual, expected));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -102,6 +102,27 @@ TEST( matrix, stack )
|
|||
EQUALITY(C,AB);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, column )
|
||||
{
|
||||
Matrix A = Matrix_(4, 7,
|
||||
-1., 0., 1., 0., 0., 0., -0.2,
|
||||
0., -1., 0., 1., 0., 0., 0.3,
|
||||
1., 0., 0., 0., -1., 0., 0.2,
|
||||
0., 1., 0., 0., 0., -1., -0.1);
|
||||
Vector a1 = column(A, 0);
|
||||
Vector exp1 = Vector_(4, -1., 0., 1., 0.);
|
||||
CHECK(assert_equal(a1, exp1));
|
||||
|
||||
Vector a2 = column(A, 3);
|
||||
Vector exp2 = Vector_(4, 0., 1., 0., 0.);
|
||||
CHECK(assert_equal(a2, exp2));
|
||||
|
||||
Vector a3 = column(A, 6);
|
||||
Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1);
|
||||
CHECK(assert_equal(a3, exp3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, zeros )
|
||||
{
|
||||
|
|
@ -509,109 +530,29 @@ TEST( matrix, svd )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, whouse_subs )
|
||||
TEST( matrix, weighted_elimination )
|
||||
{
|
||||
// create the system
|
||||
Matrix A(2,2);
|
||||
A(0,0) = 1.0; A(0,1) = 3.0;
|
||||
A(1,0) = 2.0; A(1,1) = 4.0;
|
||||
// create a matrix to eliminate - assume augmented
|
||||
Matrix A = Matrix_(4, 7,
|
||||
-1., 0., 1., 0., 0., 0., -0.2,
|
||||
0., -1., 0., 1., 0., 0., 0.3,
|
||||
1., 0., 0., 0., -1., 0., 0.2,
|
||||
0., 1., 0., 0., 0., -1., -0.1);
|
||||
Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
|
||||
|
||||
// Vector to eliminate
|
||||
Vector a(2);
|
||||
a(0) = 1.0; a(1) = 2.0;
|
||||
|
||||
Vector tau(2); //correspond to sigmas = [0.1 0.2]
|
||||
tau(0) = 100.; tau(1) = 25.;
|
||||
|
||||
// find the pseudoinverse
|
||||
Vector pseudo = whouse_solve(a, tau);
|
||||
|
||||
// substitute
|
||||
int row = 0; // eliminating the first column
|
||||
whouse_subs(A, row, a, pseudo);
|
||||
|
||||
// create expected value
|
||||
Matrix exp(2,2);
|
||||
exp(0,0) = 1.0; exp(0,1) = 3.0;
|
||||
exp(1,0) = 0.0; exp(1,1) = -1.0;
|
||||
// perform elimination
|
||||
Matrix actual; Vector newSigmas;
|
||||
boost::tie(actual, newSigmas) = weighted_eliminate(A, sigmas);
|
||||
|
||||
// verify
|
||||
CHECK(assert_equal(A, exp));
|
||||
Matrix expected = Matrix_(4, 7,
|
||||
1., 0.,-0.2, 0.,-0.8, 0., 0.2,
|
||||
0., 1., 0.,-0.2, 0.,-0.8,-0.14,
|
||||
0., 0., 1., 0., -1., 0., 0.0,
|
||||
0., 0., 0., 1., 0., -1., 0.2);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, whouse_subs_multistep )
|
||||
{
|
||||
// update two matrices
|
||||
double sigma1 = 0.2; double tau1 = 1/(sigma1*sigma1);
|
||||
double sigma2 = 0.1; double tau2 = 1/(sigma2*sigma2);
|
||||
|
||||
Matrix Ax2 = Matrix_(4,2,
|
||||
// x2
|
||||
-1., 0.,
|
||||
+0.,-1.,
|
||||
1., 0.,
|
||||
+0.,1.
|
||||
);
|
||||
|
||||
Matrix Al1 = Matrix_(4,2,
|
||||
// l1
|
||||
1., 0.,
|
||||
0., 1.,
|
||||
0., 0.,
|
||||
0., 0.
|
||||
);
|
||||
|
||||
// Eliminating x2 - step 1
|
||||
Vector a1 = Vector_(4, -1., 0., 1., 0.);
|
||||
Vector tau = Vector_(4, tau1, tau1, tau2, tau2);
|
||||
Vector pseudo1 = whouse_solve(a1, tau);
|
||||
|
||||
size_t row = 0;
|
||||
whouse_subs(Ax2, row, a1, pseudo1);
|
||||
whouse_subs(Al1, row, a1, pseudo1);
|
||||
|
||||
// verify first update
|
||||
Matrix Ax2_exp = Matrix_(4,2,
|
||||
-1., 0.,
|
||||
+0.,-1.,
|
||||
+0., 0.,
|
||||
+0.,1.
|
||||
);
|
||||
CHECK(assert_equal(Ax2, Ax2_exp, 1e-4));
|
||||
Matrix Al1_exp = Matrix_(4,2,
|
||||
// l1
|
||||
1., 0.,
|
||||
0., 1.,
|
||||
0.2, 0.,
|
||||
0., 0.
|
||||
);
|
||||
CHECK(assert_equal(Al1, Al1_exp, 1e-4));
|
||||
|
||||
// Eliminating x2 - step 2
|
||||
Vector a2 = Vector_(3, -1., 0., 1.);
|
||||
Vector tauR2 = sub(tau,1,4);
|
||||
Vector pseudo2 = whouse_solve(a2, tauR2);
|
||||
|
||||
row = 1;
|
||||
whouse_subs(Ax2, row, a2, pseudo2);
|
||||
whouse_subs(Al1, row, a2, pseudo2);
|
||||
|
||||
// verify second update
|
||||
Ax2_exp = Matrix_(4,2,
|
||||
-1., 0.,
|
||||
+0.,-1.,
|
||||
+0., 0.,
|
||||
+0., 0.
|
||||
);
|
||||
CHECK(assert_equal(Ax2, Ax2_exp, 1e-4));
|
||||
Al1_exp = Matrix_(4,2,
|
||||
1., 0.,
|
||||
0., 1.,
|
||||
0.2, 0.,
|
||||
0., 0.2
|
||||
);
|
||||
CHECK(assert_equal(Al1, Al1_exp, 1e-4)); //fails
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -127,69 +127,28 @@ TEST( TestVector, concatVectors)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, whouse_solve )
|
||||
TEST( TestVector, weightedPseudoinverse )
|
||||
{
|
||||
// column from a matrix
|
||||
Vector x(2);
|
||||
x(0) = 1.0; x(1) = 2.0;
|
||||
|
||||
// create precisions - correspond to sigmas = [0.1 0.2]
|
||||
Vector tau(2);
|
||||
tau(0) = 100; tau(1) = 25;
|
||||
// create sigmas
|
||||
Vector sigmas(2);
|
||||
sigmas(0) = 0.1; sigmas(1) = 0.2;
|
||||
|
||||
// perform solve
|
||||
Vector act = whouse_solve(x, tau);
|
||||
Vector act; double precision;
|
||||
boost::tie(act, precision) = weightedPseudoinverse(x, sigmas);
|
||||
|
||||
// construct expected
|
||||
Vector exp(2);
|
||||
exp(0) = 0.5; exp(1) = 0.25;
|
||||
double expPrecision = 200.0;
|
||||
|
||||
// verify
|
||||
CHECK(assert_equal(act, exp));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, whouse_subs_vector )
|
||||
{
|
||||
// vector to update
|
||||
Vector b(2);
|
||||
b(0) = 5; b(1) = 6;
|
||||
|
||||
// Vector to eliminate
|
||||
Vector a(2);
|
||||
a(0) = 1.0; a(1) = 2.0;
|
||||
|
||||
Vector tau(2); //correspond to sigmas = [0.1 0.2]
|
||||
tau(0) = 100; tau(1) = 25;
|
||||
|
||||
// find the pseudoinverse
|
||||
Vector pseudo = whouse_solve(a, tau);
|
||||
|
||||
// substitute
|
||||
int row = 0; // eliminating the first column
|
||||
whouse_subs(b, row, a, pseudo);
|
||||
|
||||
// create expected value
|
||||
Vector exp(2);
|
||||
exp(0) = 5; exp(1) = -2.0;
|
||||
|
||||
// verify
|
||||
CHECK(assert_equal(b, exp, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, whouse_subs_vector2 )
|
||||
{
|
||||
double sigma1 = 0.2; double tau1 = 1/(sigma1*sigma1);
|
||||
double sigma2 = 0.1; double tau2 = 1/(sigma2*sigma2);
|
||||
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
|
||||
|
||||
Vector a1 = Vector_(4, -1., 0., 1., 0.);
|
||||
Vector tau = Vector_(4, tau1, tau1, tau2, tau2);
|
||||
Vector pseudo1 = whouse_solve(a1, tau);
|
||||
|
||||
Vector expected = Vector_(4,-0.2, 0., 0.8, 0.);
|
||||
CHECK(assert_equal(pseudo1, expected, 1e-4));
|
||||
CHECK(assert_equal(act, exp));
|
||||
CHECK(fabs(expPrecision-precision) < 1e-5);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue