12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'unified_constraints' 

Conflicts:
	gtsam/linear/NoiseModel.cpp
	gtsam/linear/NoiseModel.h
	gtsam/linear/tests/testNoiseModel.cpp
release/4.3a0
Alex Cunningham 2011-11-09 22:15:40 +00:00
parent 70afdfb7d3
commit 98410ca5c9
19 changed files with 418 additions and 803 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

229
.cproject
View File

@ -375,6 +375,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -401,7 +409,6 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -409,7 +416,6 @@
</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>
@ -457,7 +463,6 @@
</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>
@ -465,7 +470,6 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -473,7 +477,6 @@
</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>
@ -489,20 +492,11 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -529,6 +523,7 @@
</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>
@ -600,6 +595,7 @@
</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>
@ -607,6 +603,7 @@
</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>
@ -614,6 +611,7 @@
</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>
@ -621,6 +619,7 @@
</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>
@ -628,6 +627,7 @@
</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>
@ -681,22 +681,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -713,6 +697,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -737,15 +737,7 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" 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="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
@ -753,14 +745,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build_wrap" 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="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -801,7 +785,15 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="all" path="build_wrap" 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="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
@ -809,6 +801,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build_wrap" 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="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1025,6 +1025,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearEquality.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearEquality.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearEqualityConstraint.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearEqualityConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1123,7 +1139,6 @@
</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>
@ -1531,6 +1546,7 @@
</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>
@ -1570,6 +1586,7 @@
</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>
@ -1577,6 +1594,7 @@
</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>
@ -1824,6 +1842,7 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1845,46 +1864,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>
@ -1981,6 +1960,62 @@
<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/wrap" 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="install" path="build/wrap" 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="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2021,22 +2056,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" 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="install" path="build/wrap" 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>
</buildTargets>
</storageModule>
</cproject>

32
gtsam/base/Matrix.cpp
View File

@ -500,27 +500,43 @@ Matrix collect(size_t nrMatrices, ...)
/* ************************************************************************* */
// row scaling, in-place
void vector_scale_inplace(const Vector& v, Matrix& A) {
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
const size_t m = A.rows();
for (size_t i=0; i<m; ++i)
A.row(i) *= v(i);
if (inf_mask) {
for (size_t i=0; i<m; ++i) {
const double& vi = v(i);
if (!isnan(vi) && !isinf(vi))
A.row(i) *= vi;
}
} else {
for (size_t i=0; i<m; ++i)
A.row(i) *= v(i);
}
}
/* ************************************************************************* */
// row scaling
Matrix vector_scale(const Vector& v, const Matrix& A) {
Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask) {
Matrix M(A);
vector_scale_inplace(v, M);
vector_scale_inplace(v, M, inf_mask);
return M;
}
/* ************************************************************************* */
// column scaling
Matrix vector_scale(const Matrix& A, const Vector& v) {
Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
Matrix M(A);
const size_t n = A.cols();
for (size_t j=0; j<n; ++j)
M.col(j) *= v(j);
if (inf_mask) {
for (size_t j=0; j<n; ++j) {
const double& vj = v(j);
if (!isnan(vj) && !isinf(vj))
M.col(j) *= vj;
}
} else {
for (size_t j=0; j<n; ++j)
M.col(j) *= v(j);
}
return M;
}

7
gtsam/base/Matrix.h
View File

@ -377,10 +377,11 @@ Matrix collect(size_t nrMatrices, ...);
* scales a matrix row or column by the values in a vector
* Arguments (Matrix, Vector) scales the columns,
* (Vector, Matrix) scales the rows
* @param inf_mask when true, will not scale with a NaN or inf value
*/
void vector_scale_inplace(const Vector& v, Matrix& A); // row
Matrix vector_scale(const Vector& v, const Matrix& A); // row
Matrix vector_scale(const Matrix& A, const Vector& v); // column
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // column
/**
* skew symmetric matrix returns this:

4
gtsam/base/Vector.cpp
View File

@ -272,8 +272,8 @@ Vector ediv_(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
Vector c(n);
for( size_t i = 0; i < n; i++ ) {
double ai = a(i), bi = b(i);
c(i) = (bi==0.0 && ai==0.0) ? 0.0 : a(i)/b(i);
const double &ai = a(i), &bi = b(i);
c(i) = (bi==0.0 && ai==0.0) ? 0.0 : ai/bi;
}
return c;
}

38
gtsam/base/tests/testMatrix.cpp
View File

@ -337,6 +337,44 @@ TEST( matrix, scale_rows )
EXPECT(assert_equal(actual, expected));
}
/* ************************************************************************* */
TEST( matrix, scale_rows_mask )
{
Matrix A(3, 4);
A(0, 0) = 1.;
A(0, 1) = 1.;
A(0, 2) = 1.;
A(0, 3) = 1.;
A(1, 0) = 1.;
A(1, 1) = 1.;
A(1, 2) = 1.;
A(1, 3) = 1.;
A(2, 0) = 1.;
A(2, 1) = 1.;
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = Vector_(3, 2., std::numeric_limits<double>::infinity(), 4.);
Matrix actual = vector_scale(v, A, true);
Matrix expected(3, 4);
expected(0, 0) = 2.;
expected(0, 1) = 2.;
expected(0, 2) = 2.;
expected(0, 3) = 2.;
expected(1, 0) = 1.;
expected(1, 1) = 1.;
expected(1, 2) = 1.;
expected(1, 3) = 1.;
expected(2, 0) = 4.;
expected(2, 1) = 4.;
expected(2, 2) = 4.;
expected(2, 3) = 4.;
EXPECT(assert_equal(actual, expected));
}
/* ************************************************************************* */
TEST( matrix, equal )
{

46
gtsam/linear/NoiseModel.cpp
View File

@ -291,13 +291,35 @@ Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector&
/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
gtsam::print(sigmas_, name + ": constrained sigmas");
gtsam::print(mu_, name + ": constrained mu");
}
/* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const {
// ediv_ does the right thing with the errors
// FIXME: solve this more effectively
return ediv_(v, sigmas_);
// return ediv_(v, sigmas_);
const Vector& a = v;
const Vector& b = sigmas_;
size_t n = a.size();
assert (b.size()==a.size());
Vector c(n);
for( size_t i = 0; i < n; i++ ) {
const double& ai = a(i), &bi = b(i);
c(i) = (bi==0.0) ? ai : ai/bi;
}
return c;
}
/* ************************************************************************* */
double Constrained::distance(const Vector& v) const {
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
for (size_t i=0; i<dim_; ++i) { // add mu weights on constrained variables
if (isinf(w[i])) // whiten makes constrained variables infinite
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
if (isnan(w[i])) // ensure no other invalid values make it through
w[i] = v[i];
}
return w.dot(w);
}
/* ************************************************************************* */
@ -311,14 +333,23 @@ double Constrained::distance(const Vector& v) const {
/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const {
// FIXME: replace with pass-through
throw logic_error("noiseModel::Constrained cannot Whiten");
// selective scaling
return vector_scale(invsigmas(), H, true);
}
/* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const {
// FIXME: replace with pass-through
throw logic_error("noiseModel::Constrained cannot Whiten");
// selective scaling
vector_scale_inplace(invsigmas(), H, true);
}
/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit() const {
Vector sigmas = ones(dim());
for (size_t i=0; i<dim(); ++i)
if (this->sigmas_(i) == 0.0)
sigmas(i) = 0.0;
return MixedSigmas(mu_, sigmas);
}
/* ************************************************************************* */
@ -401,7 +432,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
}
toc(4, "constrained_QR write back into Ab");
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
// Must include mus, as the defaults might be higher, resulting in non-convergence
return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
}
/* ************************************************************************* */

27
gtsam/linear/NoiseModel.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
/// Dimensionality
inline size_t dim() const { return dim_;}
virtual void print(const std::string& name) const = 0;
virtual void print(const std::string& name = "") const = 0;
virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
@ -327,8 +327,11 @@ namespace gtsam {
* All other Gaussian models are guaranteed to have a non-singular square-root
* information matrix, but this class is specifically equipped to deal with
* singular noise models, specifically: whiten will return zero on those
* components that have zero sigma *and* zero error, infinity otherwise.
* FIXME: the "otherwise return infinity" does not solve anything
* components that have zero sigma *and* zero error, unchanged otherwise.
*
* While a hard constraint may seem to be a case in which there is infinite error,
* we do not ever produce an error value of infinity to allow for constraints
* to actually be optimized rather than self-destructing if not initialized correctly.
*
* The distance function in this function provides an error model
* for a penalty function with a scaling function, assuming a mask of
@ -387,8 +390,10 @@ namespace gtsam {
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
* TODO: allow for mu
*/
static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
return shared_ptr(new Constrained(mu, esqrt(variances)));
}
static shared_ptr MixedVariances(const Vector& variances) {
return shared_ptr(new Constrained(esqrt(variances)));
}
@ -398,6 +403,9 @@ namespace gtsam {
* precisions, some of which might be inf
* TODO: allow for mu
*/
static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
return MixedVariances(mu, reciprocal(precisions));
}
static shared_ptr MixedPrecisions(const Vector& precisions) {
return MixedVariances(reciprocal(precisions));
}
@ -429,9 +437,8 @@ namespace gtsam {
/// Calculates error vector with weights applied
virtual Vector whiten(const Vector& v) const;
// Whitening Jacobians does not make sense for possibly constrained
// noise model and will throw an exception.
// FIXME: change to allow for use a normal linearization function
/// Whitening functions will perform partial whitening on rows
/// with a non-zero sigma. Other rows remain untouched.
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
@ -446,6 +453,12 @@ namespace gtsam {
*/
virtual bool isConstrained() const {return true;}
/**
* Returns a Unit version of a constrained noisemodel in which
* constrained sigmas remain constrained and the rest are unit scaled
*/
shared_ptr unit() const;
private:
/** Serialization function */
friend class boost::serialization::access;

6
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -187,7 +187,8 @@ TEST(NoiseModel, ConstrainedMixed )
Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
infeasible = Vector_(3, 1.0, 1.0, 1.0);
Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
EXPECT(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible)));
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
EXPECT(assert_equal(Vector_(3, 0.5, 1.0, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
@ -201,7 +202,8 @@ TEST(NoiseModel, ConstrainedAll )
infeasible = Vector_(3, 1.0, 1.0, 1.0);
Constrained::shared_ptr i = Constrained::All(3);
EXPECT(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible)));
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
EXPECT(assert_equal(Vector_(3, 1.0, 1.0, 1.0),i->whiten(infeasible)));
EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);

561
gtsam/nonlinear/NonlinearConstraint.h
View File

@ -11,8 +11,7 @@
/*
* @file NonlinearConstraint.h
* @brief Implements nonlinear constraints that can be linearized using
* direct linearization and solving through a quadratic merit function
* @brief Implements simple cases of constraints
* @author Alex Cunningham
*/
@ -22,566 +21,48 @@
namespace gtsam {
/**
* Base class for nonlinear constraints
* This allows for both equality and inequality constraints,
* where equality constraints are active all the time (even slightly
* nonzero constraint functions will still be active - inequality
* constraints should be sure to force to actual zero)
*
* Nonlinear constraints evaluate their error as a part of a quadratic
* error function: ||h(x)-z||^2 + mu * ||c(x)|| where mu is a gain
* on the constraint function that should be made high enough to be
* significant
*/
template <class VALUES>
class NonlinearConstraint : public NonlinearFactor<VALUES> {
protected:
typedef NonlinearConstraint<VALUES> This;
typedef NonlinearFactor<VALUES> Base;
typedef Factor<Symbol> BaseFactor;
/** default constructor to allow for serialization */
NonlinearConstraint() {}
double mu_; /// gain for quadratic merit function
size_t dim_; /// dimension of the constraint
public:
/** Constructor - sets the cost function and the lagrange multipliers
* @param dim is the dimension of the factor
* @param keys is a boost::tuple containing the keys, e.g. \c make_tuple(key1,key2,key3)
* @param mu is the gain used at error evaluation (forced to be positive)
*/
template<class U1, class U2>
NonlinearConstraint(const boost::tuples::cons<U1,U2>& keys, size_t dim, double mu = 1000.0)
: Base(keys), mu_(fabs(mu)), dim_(dim) {
}
/**
* Constructor
* @param keys The variables involved in this factor
*/
template<class ITERATOR>
NonlinearConstraint(ITERATOR beginKeys, ITERATOR endKeys, size_t dim, double mu = 1000.0)
: Base(beginKeys, endKeys), mu_(fabs(mu)), dim_(dim) {
}
virtual ~NonlinearConstraint() {}
/** returns the gain mu */
double mu() const { return mu_; }
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const {
return dim_;
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint " << s << " mu: " << this->mu_ << std::endl;
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor<VALUES>& f, double tol=1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return BaseFactor::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol) && dim_ == p->dim_;
}
/** error function - returns the quadratic merit function */
virtual double error(const VALUES& c) const {
if (active(c))
return mu_ * unwhitenedError(c).squaredNorm();
else
return 0.0;
}
/**
* active set check, defines what type of constraint this is
*
* In an inequality/bounding constraint, this active() returns true
* when the constraint is *NOT* fulfilled.
* @return true if the constraint is active
*/
virtual bool active(const VALUES& c) const=0;
/**
* Error function \f$ z-c(x) \f$.
* Override this method to finish implementing an N-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
*/
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
/**
* Linearizes around a given config
* @param config is the values structure
* @return a combined linear factor containing both the constraint and the constraint factor
*/
virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
if (!active(c))
return boost::shared_ptr<JacobianFactor>();
// Create the set of terms - Jacobians for each index
Vector b;
// Call evaluate error to get Jacobians and b vector
std::vector<Matrix> A(this->size());
b = -unwhitenedError(c, A);
std::vector<std::pair<Index, Matrix> > terms(this->size());
// Fill in terms
for(size_t j=0; j<this->size(); ++j) {
terms[j].first = ordering[this->keys()[j]];
terms[j].second.swap(A[j]);
}
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, noiseModel::Constrained::All(dim_)));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(mu_);
ar & BOOST_SERIALIZATION_NVP(dim_);
}
};
/* ************************************************************************* */
/**
* A unary constraint that defaults to an equality constraint
*/
template <class VALUES, class KEY>
class NonlinearConstraint1 : public NonlinearConstraint<VALUES> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearConstraint1<VALUES,KEY> This;
typedef NonlinearConstraint<VALUES> Base;
/** default constructor to allow for serialization */
NonlinearConstraint1() {}
/** key for the constrained variable */
KEY key_;
public:
/**
* Basic constructor
* @param key is the identifier for the variable constrained
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint1(const KEY& key, size_t dim, double mu = 1000.0)
: Base(make_tuple(key), dim, mu), key_(key) { }
virtual ~NonlinearConstraint1() {}
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X& x1 = x[key_];
if(H) {
return evaluateError(x1, (*H)[0]);
} else {
return evaluateError(x1);
}
} else {
return zero(this->dim());
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearConstraint("
<< (std::string) this->key_ << ")," <<
" mu = " << this->mu_ <<
" dim = " << this->dim_ << std::endl;
}
/**
* Override this method to finish implementing a unary factor.
* If the optional Matrix reference argument is specified, it should compute
* both the function evaluation and its derivative in X.
*/
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
}
};
/* ************************************************************************* */
/**
* Unary Equality constraint - simply forces the value of active() to true
*/
template <class VALUES, class KEY>
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<VALUES, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint1<VALUES,KEY> This;
typedef NonlinearConstraint1<VALUES,KEY> Base;
/** default constructor to allow for serialization */
NonlinearEqualityConstraint1() {}
public:
NonlinearEqualityConstraint1(const KEY& key, size_t dim, double mu = 1000.0)
: Base(key, dim, mu) {}
virtual ~NonlinearEqualityConstraint1() {}
/** Always active, so fixed value for active() */
virtual bool active(const VALUES& c) const { return true; }
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearEqualityConstraint("
<< (std::string) this->key_ << ")," <<
" mu = " << this->mu_ <<
" dim = " << this->dim_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint1",
boost::serialization::base_object<Base>(*this));
}
};
/* ************************************************************************* */
/**
* A binary constraint with arbitrary cost and jacobian functions
*/
template <class VALUES, class KEY1, class KEY2>
class NonlinearConstraint2 : public NonlinearConstraint<VALUES> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
protected:
typedef NonlinearConstraint2<VALUES,KEY1,KEY2> This;
typedef NonlinearConstraint<VALUES> Base;
/** default constructor to allow for serialization */
NonlinearConstraint2() {}
/** keys for the constrained variables */
KEY1 key1_;
KEY2 key2_;
public:
/**
* Basic constructor
* @param key1 is the identifier for the first variable constrained
* @param key2 is the identifier for the second variable constrained
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0) :
Base(make_tuple(key1, key2), dim, mu), key1_(key1), key2_(key2) { }
virtual ~NonlinearConstraint2() {}
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
if(H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else {
return evaluateError(x1, x2);
}
} else {
return zero(this->dim());
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearConstraint("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ")," <<
" mu = " << this->mu_ <<
" dim = " << this->dim_ << std::endl;
}
/**
* Override this method to finish implementing a binary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2).
*/
virtual Vector
evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
};
/* ************************************************************************* */
/**
* Binary Equality constraint - simply forces the value of active() to true
*/
template <class VALUES, class KEY1, class KEY2>
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<VALUES, KEY1, KEY2> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
protected:
typedef NonlinearEqualityConstraint2<VALUES,KEY1,KEY2> This;
typedef NonlinearConstraint2<VALUES,KEY1,KEY2> Base;
/** default constructor to allow for serialization */
NonlinearEqualityConstraint2() {}
public:
NonlinearEqualityConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0)
: Base(key1, key2, dim, mu) {}
virtual ~NonlinearEqualityConstraint2() {}
/** Always active, so fixed value for active() */
virtual bool active(const VALUES& c) const { return true; }
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearEqualityConstraint("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ")," <<
" mu = " << this->mu_ <<
" dim = " << this->dim_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint2",
boost::serialization::base_object<Base>(*this));
}
};
/* ************************************************************************* */
/**
* A ternary constraint
*/
template <class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearConstraint3 : public NonlinearConstraint<VALUES> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
protected:
typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> This;
typedef NonlinearConstraint<VALUES> Base;
/** default constructor to allow for serialization */
NonlinearConstraint3() {}
/** keys for the constrained variables */
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
public:
/**
* Basic constructor
* @param key1 is the identifier for the first variable constrained
* @param key2 is the identifier for the second variable constrained
* @param key3 is the identifier for the second variable constrained
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3,
size_t dim, double mu = 1000.0) :
Base(make_tuple(key1, key2, key3), dim, mu), key1_(key1), key2_(key2), key3_(key3) { }
virtual ~NonlinearConstraint3() {}
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
const X3& x3 = x[key3_];
if(H) {
return evaluateError(x1, x2, x3, (*H)[0], (*H)[1], (*H)[2]);
} else {
return evaluateError(x1, x2, x3);
}
} else {
return zero(this->dim());
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearConstraint("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ")," <<
" mu = " << this->mu_ <<
" dim = " << this->dim_ << std::endl;
}
/**
* Override this method to finish implementing a trinary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
*/
virtual Vector
evaluateError(const X1&, const X2&, const X3&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
}
};
/* ************************************************************************* */
/**
* Ternary Equality constraint - simply forces the value of active() to true
*/
template <class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<VALUES, KEY1, KEY2, KEY3> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
protected:
typedef NonlinearEqualityConstraint3<VALUES,KEY1,KEY2,KEY3> This;
typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> Base;
/** default constructor to allow for serialization */
NonlinearEqualityConstraint3() {}
public:
NonlinearEqualityConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3,
size_t dim, double mu = 1000.0)
: Base(key1, key2, key3, dim, mu) {}
virtual ~NonlinearEqualityConstraint3() {}
/** Always active, so fixed value for active() */
virtual bool active(const VALUES& c) const { return true; }
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearEqualityConstraint("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ")," <<
" mu = " << this->mu_ <<
" dim = " << this->dim_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint3",
boost::serialization::base_object<Base>(*this));
}
};
/* ************************************************************************* */
/**
* Simple unary equality constraint - fixes a value for a variable
*/
template<class VALUES, class KEY>
class NonlinearEquality1 : public NonlinearEqualityConstraint1<VALUES, KEY> {
class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint1<VALUES, KEY> Base;
typedef NonlinearFactor1<VALUES, KEY> Base;
/** default constructor to allow for serialization */
NonlinearEquality1() {}
X value_; /// fixed value for variable
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
GTSAM_CONCEPT_TESTABLE_TYPE(X);
public:
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
: Base(key1, X::Dim(), mu), value_(value) {}
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
virtual ~NonlinearEquality1() {}
/** g(x) with optional derivative */
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
const size_t p = X::Dim();
if (H1) *H1 = eye(p);
Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = eye(x1.dim());
// manifold equivalent of h(x)-z -> log(z,h(x))
return value_.localCoordinates(x1);
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearEquality("
<< (std::string) this->key_ << "),"<<
" mu = " << this->mu_ <<
" dim = " << this->dim_ << "\n";
std::cout << s << ": NonlinearEquality1("
<< (std::string) this->key_ << "),"<< "\n";
this->noiseModel_->print();
value_.print("Value");
}
@ -591,7 +72,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint1",
ar & boost::serialization::make_nvp("NonlinearFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(value_);
}
@ -600,15 +81,17 @@ private:
/* ************************************************************************* */
/**
* Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type
* be the same.
*/
template<class VALUES, class KEY>
class NonlinearEquality2 : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> {
class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base;
typedef NonlinearFactor2<VALUES, KEY, KEY> Base;
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
/** default constructor to allow for serialization */
NonlinearEquality2() {}
@ -618,7 +101,7 @@ public:
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu) {}
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}
/** g(x) with optional derivative2 */
@ -637,7 +120,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2",
ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this));
}
};

3
gtsam/nonlinear/NonlinearEquality.h
View File

@ -101,7 +101,8 @@ namespace gtsam {
/** Check if two factors are equal */
bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
if (!Base::equals(f)) return false;
return compare_(feasible_, f.feasible_);
return feasible_.equals(f.feasible_, tol) &&
fabs(error_gain_ - f.error_gain_) < tol;
}
/** actual error function calculation */

110
gtsam/nonlinear/NonlinearFactor.h
View File

@ -112,6 +112,18 @@ public:
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const = 0;
/**
* Checks whether a factor should be used based on a set of values.
* This is primarily used to implment inequality constraints that
* require a variable active set. For all others, the default implementation
* returning true solves this problem.
*
* In an inequality/bounding constraint, this active() returns true
* when the constraint is *NOT* fulfilled.
* @return true if the constraint is active
*/
virtual bool active(const VALUES& c) const { return true; }
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor>
linearize(const VALUES& c, const Ordering& ordering) const = 0;
@ -138,7 +150,7 @@ public:
* the derived class implements \f$ \mathtt{error\_vector}(x) = h(x)-z \approx A \delta x - b \f$
* This allows a graph to have factors with measurements of mixed type.
* The noise model is typically Gaussian, but robust error models are also supported.
* The noise model is typically Gaussian, but robust and constrained error models are also supported.
*/
template<class VALUES>
class NoiseModelFactor: public NonlinearFactor<VALUES> {
@ -237,7 +249,10 @@ public:
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const VALUES& c) const {
return 0.5 * noiseModel_->distance(unwhitenedError(c));
if (active(c))
return 0.5 * noiseModel_->distance(unwhitenedError(c));
else
return 0.0;
}
/**
@ -246,6 +261,9 @@ public:
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
// Only linearize if the factor is active
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
// Create the set of terms - Jacobians for each index
Vector b;
@ -253,11 +271,7 @@ public:
std::vector<Matrix> A(this->size());
b = -unwhitenedError(x, A);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if(!constrained)
this->noiseModel_->WhitenSystem(A,b);
this->noiseModel_->WhitenSystem(A,b);
std::vector<std::pair<Index, Matrix> > terms(this->size());
// Fill in terms
@ -266,9 +280,12 @@ public:
terms[j].second.swap(A[j]);
}
// TODO pass unwhitened + noise model to Gaussian factor
noiseModel::Constrained::shared_ptr constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if(constrained)
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, constrained));
new JacobianFactor(terms, b, constrained->unit()));
else
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size())));
@ -328,10 +345,16 @@ public:
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key_], (*H)[0]);
else
return evaluateError(x[key_]);
if(this->active(x)) {
const X& x1 = x[key_];
if(H) {
return evaluateError(x1, (*H)[0]);
} else {
return evaluateError(x1);
}
} else {
return zero(this->dim());
}
}
/** Print */
@ -406,10 +429,17 @@ public:
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], (*H)[0], (*H)[1]);
else
return evaluateError(x[key1_], x[key2_]);
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
if(H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else {
return evaluateError(x1, x2);
}
} else {
return zero(this->dim());
}
}
/** Print */
@ -491,10 +521,14 @@ public:
/** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
else
return evaluateError(x[key1_], x[key2_], x[key3_]);
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
else
return evaluateError(x[key1_], x[key2_], x[key3_]);
} else {
return zero(this->dim());
}
}
/** Print */
@ -585,10 +619,14 @@ public:
/** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
} else {
return zero(this->dim());
}
}
/** Print */
@ -685,10 +723,14 @@ public:
/** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
} else {
return zero(this->dim());
}
}
/** Print */
@ -792,10 +834,14 @@ public:
/** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
} else {
return zero(this->dim());
}
}
/** Print */

73
gtsam/slam/BetweenConstraint.h
View File

@ -1,73 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BetweenConstraint.h
* @brief Implements a constraint to force a between
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearConstraint.h>
namespace gtsam {
/**
* Binary between constraint - forces between to a given value
* This constraint requires the underlying type to a Lie type
*/
template<class VALUES, class KEY>
class BetweenConstraint : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base;
X measured_; /// fixed between value
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(X)
public:
typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
BetweenConstraint(const X& measured, const KEY& key1, const KEY& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu), measured_(measured) {}
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
X hx = x1.between(x2, H1, H2);
return measured_.localCoordinates(hx);
}
inline const X measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
} // \namespace gtsam

30
gtsam/slam/BetweenFactor.h
View File

@ -40,6 +40,7 @@ namespace gtsam {
T measured_; /** The measurement */
/** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T)
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
@ -106,6 +107,33 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
}; // \class BetweenFactor
/**
* Binary between constraint - forces between to a given value
* This constraint requires the underlying type to a Lie type
*
*/
template<class VALUES, class KEY>
class BetweenConstraint : public BetweenFactor<VALUES, KEY> {
public:
typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
/** Syntactic sugar for constrained version */
BetweenConstraint(const typename KEY::Value& measured,
const KEY& key1, const KEY& key2, double mu = 1000.0)
: BetweenFactor<VALUES, KEY>(key1, key2, measured,
noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("BetweenFactor",
boost::serialization::base_object<BetweenFactor<VALUES, KEY> >(*this));
}
}; // \class BetweenConstraint
} /// namespace gtsam

18
gtsam/slam/BoundingConstraint.h
View File

@ -29,9 +29,9 @@ namespace gtsam {
* a scalar for comparison.
*/
template<class VALUES, class KEY>
struct BoundingConstraint1: public NonlinearConstraint1<VALUES, KEY> {
struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
typedef typename KEY::Value X;
typedef NonlinearConstraint1<VALUES, KEY> Base;
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
double threshold_;
@ -39,7 +39,8 @@ struct BoundingConstraint1: public NonlinearConstraint1<VALUES, KEY> {
BoundingConstraint1(const KEY& key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {
Base(noiseModel::Constrained::All(1, mu), key),
threshold_(threshold), isGreaterThan_(isGreaterThan) {
}
virtual ~BoundingConstraint1() {}
@ -83,7 +84,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint1",
ar & boost::serialization::make_nvp("NonlinearFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
@ -95,11 +96,11 @@ private:
* to implement for specific systems
*/
template<class VALUES, class KEY1, class KEY2>
struct BoundingConstraint2: public NonlinearConstraint2<VALUES, KEY1, KEY2> {
struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef NonlinearConstraint2<VALUES, KEY1, KEY2> Base;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
double threshold_;
@ -107,7 +108,8 @@ struct BoundingConstraint2: public NonlinearConstraint2<VALUES, KEY1, KEY2> {
BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {}
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
threshold_(threshold), isGreaterThan_(isGreaterThan) {}
virtual ~BoundingConstraint2() {}
@ -155,7 +157,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint2",
ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);

2
gtsam/slam/Makefile.am
View File

@ -40,7 +40,7 @@ headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h
headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h
# Generic constraint headers
headers += BetweenConstraint.h BoundingConstraint.h
headers += BoundingConstraint.h
# 2D Pose SLAM
sources += pose2SLAM.cpp dataset.cpp

2
gtsam/slam/simulated2DConstraints.h
View File

@ -22,7 +22,7 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/NonlinearConstraint.h>
#include <gtsam/slam/BetweenConstraint.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BoundingConstraint.h>
#include <gtsam/slam/simulated2D.h>

10
tests/testBoundingConstraint.cpp
View File

@ -96,8 +96,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) {
EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config), tol);
EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
}
/* ************************************************************************* */
@ -111,8 +111,8 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(10.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(10.0, constraint4.error(config), tol);
EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
}
/* ************************************************************************* */
@ -224,7 +224,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
config1.update(key2, pt4);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(1.0*mu, rangeBound.error(config1), tol);
EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
}
/* ************************************************************************* */

16
tests/testNonlinearEqualityConstraint.cpp
View File

@ -56,7 +56,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol));
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol);
}
/* ************************************************************************* */
@ -88,7 +88,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
// ensure that the hard constraint overrides the soft constraint
Point2 truth_pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
double mu = 10.0;
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
@ -103,10 +103,16 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
shared_values initValues(new simulated2D::Values());
initValues->insert(key, badPt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
// verify error values
EXPECT(constraint->active(*initValues));
simulated2D::Values expected;
expected.insert(key, truth_pt);
CHECK(assert_equal(expected, *actual, tol));
EXPECT(constraint->active(expected));
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
EXPECT(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
@ -132,7 +138,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(2000.0, constraint.error(config2), tol);
EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
}
/* ************************************************************************* */

7
tests/testNonlinearFactor.cpp
View File

@ -209,7 +209,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
// create expected
Ordering ord(*config.orderingArbitrary());
Vector b = Vector_(2, 0., -3.);
JacobianFactor expected(ord["x1"], eye(2), b, constraint);
JacobianFactor expected(ord["x1"], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
@ -229,8 +229,9 @@ TEST( NonlinearFactor, linearize_constraint2 )
// create expected
Ordering ord(*config.orderingArbitrary());
Vector b = Vector_(2, -3., -3.);
JacobianFactor expected(ord["x1"], -1*eye(2), ord["l1"], eye(2), b, constraint);
Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0);
Vector b = Vector_(2, -15., -3.);
JacobianFactor expected(ord["x1"], -1*A, ord["l1"], A, b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}