NonlinearEquality factor is now working for generic configs. Currently, the feasible point must be specified in the form of a full config with the point in it, but this avoid needing to increase the requirements on the configs.
Alex Cunningham
parent
cba4a536bb
commit
67c2ea8263
90
.cproject
90
.cproject
|
|
@ -1,4 +1,4 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
|
|
@ -298,20 +298,6 @@
|
|||
</storageModule>
|
||||
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
|
||||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-k</buildArguments>
|
||||
|
|
@ -322,6 +308,7 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -337,7 +324,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,21 +331,22 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianConditional.run</buildTarget>
|
||||
<buildTarget>testConditionalGaussian.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -367,6 +354,7 @@
|
|||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -374,29 +362,29 @@
|
|||
</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>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
|
|
@ -404,6 +392,7 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -411,7 +400,6 @@
|
|||
</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>
|
||||
|
|
@ -419,7 +407,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>
|
||||
|
|
@ -427,27 +414,31 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>timeGaussianFactor.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>timeLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>timeLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -455,7 +446,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>
|
||||
|
|
@ -463,6 +453,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -470,7 +461,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>
|
||||
|
|
@ -478,6 +468,7 @@
|
|||
</target>
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -485,6 +476,7 @@
|
|||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -492,23 +484,29 @@
|
|||
</target>
|
||||
<target name="testInference.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testInference.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVSLAMFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMFactorGraph.run</buildTarget>
|
||||
<buildTarget>testNonlinearEquality.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testVSLAMGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -516,7 +514,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>
|
||||
|
|
@ -524,7 +521,22 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -111,8 +111,8 @@ timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitL
|
|||
# Nonlinear inference
|
||||
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
|
||||
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
|
||||
sources += NonlinearFactor.cpp
|
||||
sources += NonlinearEquality.cpp
|
||||
sources += NonlinearFactor.cpp
|
||||
sources += NonlinearEquality.h
|
||||
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality
|
||||
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
|
||||
testNonlinearFactor_LDADD = libgtsam.la
|
||||
|
|
|
|||
|
|
@ -1,9 +0,0 @@
|
|||
/*
|
||||
* @file NonlinearEquality.cpp
|
||||
* @brief
|
||||
* @author alexgc
|
||||
*/
|
||||
|
||||
#include "NonlinearEquality.h"
|
||||
|
||||
|
||||
|
|
@ -6,6 +6,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include "NonlinearFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -16,10 +17,74 @@ namespace gtsam {
|
|||
* Throws an error at linearization if the constraints are not met.
|
||||
*/
|
||||
template<class Config>
|
||||
class NonlinearEquality : public NonlinearFactor<Config>{
|
||||
class NonlinearEquality : public NonlinearFactor<Config> {
|
||||
private:
|
||||
|
||||
// node to constrain
|
||||
std::string key_;
|
||||
|
||||
// config containing the necessary feasible point
|
||||
Config feasible_;
|
||||
|
||||
// dimension of the variable
|
||||
size_t dim_;
|
||||
|
||||
public:
|
||||
NonlinearEquality();
|
||||
virtual ~NonlinearEquality();
|
||||
|
||||
/**
|
||||
* Function that compares a value from a config with
|
||||
* another to determine whether a linearization point is
|
||||
* a feasible point.
|
||||
* @param key is the identifier for the key
|
||||
* @param feasible is the value which is constrained
|
||||
* @param input is the config to be tested for feasibility
|
||||
* @return true if the linearization point is feasible
|
||||
*/
|
||||
bool (*compare_)(const std::string& key, const Config& feasible, const Config& input);
|
||||
|
||||
/** Constructor */
|
||||
NonlinearEquality(const std::string& key,
|
||||
const Config& feasible,
|
||||
size_t dim,
|
||||
bool (*compare)(const std::string& key,
|
||||
const Config& feasible,
|
||||
const Config& input))
|
||||
: key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
|
||||
|
||||
}
|
||||
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << "Constraint: " << s << " on [" << key_ << "]\n";
|
||||
feasible_.print("Feasible Point");
|
||||
std::cout << "Variable Dimension: " << dim_ << std::endl;
|
||||
}
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const Factor<Config>& f, double tol=1e-9) const {
|
||||
const NonlinearEquality<Config>* p = dynamic_cast<const NonlinearEquality<Config>*> (&f);
|
||||
if (p == NULL) return false;
|
||||
if (key_ != p->key_) return false;
|
||||
if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value
|
||||
return dim_ == p->dim_;
|
||||
}
|
||||
|
||||
/** error function */
|
||||
inline Vector error_vector(const VectorConfig& c) const {
|
||||
if (!compare_(key_, feasible_, c))
|
||||
return repeat(dim_, 1.0/0.0); // set error to infinity if not equal
|
||||
else
|
||||
return zero(dim_); // set error to zero if equal
|
||||
}
|
||||
|
||||
/** linearize a nonlinear constraint into a linear constraint */
|
||||
boost::shared_ptr<GaussianFactor> linearize(const VectorConfig& c) const {
|
||||
if (!compare_(key_, feasible_, c)) {
|
||||
throw std::invalid_argument("Linearization point not feasible for " + key_ + "!");
|
||||
} else {
|
||||
GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_), zero(dim_), 0.0));
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,14 +3,100 @@
|
|||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "VectorConfig.h"
|
||||
#include "NonlinearEquality.h"
|
||||
|
||||
TEST ( NonlinearEquality, construction ) {
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_nle;
|
||||
|
||||
bool vector_compare(const std::string& key,
|
||||
const VectorConfig& feasible,
|
||||
const VectorConfig& input) {
|
||||
Vector feas, lin;
|
||||
feas = feasible[key];
|
||||
lin = input[key];
|
||||
return equal_with_abs_tol(lin, feas, 1e-5);
|
||||
}
|
||||
|
||||
TEST ( NonlinearEquality, linearization ) {
|
||||
string key = "x";
|
||||
Vector value = Vector_(2, 1.0, 2.0);
|
||||
VectorConfig feasible, linearize;
|
||||
feasible.insert(key, value);
|
||||
linearize.insert(key, value);
|
||||
|
||||
// create a nonlinear equality constraint
|
||||
shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
|
||||
|
||||
// check linearize
|
||||
GaussianFactor expLF(key, eye(2), zero(2), 0.0);
|
||||
GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
|
||||
CHECK(assert_equal(*actualLF, expLF));
|
||||
}
|
||||
|
||||
TEST ( NonlinearEquality, linearization_fail ) {
|
||||
string key = "x";
|
||||
Vector value = Vector_(2, 1.0, 2.0);
|
||||
Vector wrong = Vector_(2, 3.0, 4.0);
|
||||
VectorConfig feasible, bad_linearize;
|
||||
feasible.insert(key, value);
|
||||
bad_linearize.insert(key, wrong);
|
||||
|
||||
// create a nonlinear equality constraint
|
||||
shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
|
||||
|
||||
// check linearize to ensure that it fails for bad linearization points
|
||||
try {
|
||||
GaussianFactor::shared_ptr actualLF = nle->linearize(bad_linearize);
|
||||
CHECK(false);
|
||||
} catch (std::invalid_argument) {
|
||||
CHECK(true);
|
||||
}
|
||||
}
|
||||
|
||||
TEST ( NonlinearEquality, error ) {
|
||||
string key = "x";
|
||||
Vector value = Vector_(2, 1.0, 2.0);
|
||||
Vector wrong = Vector_(2, 3.0, 4.0);
|
||||
VectorConfig feasible, bad_linearize;
|
||||
feasible.insert(key, value);
|
||||
bad_linearize.insert(key, wrong);
|
||||
|
||||
// create a nonlinear equality constraint
|
||||
shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
|
||||
|
||||
// check error function outputs
|
||||
Vector actual = nle->error_vector(feasible);
|
||||
CHECK(assert_equal(actual, zero(2)));
|
||||
|
||||
actual = nle->error_vector(bad_linearize);
|
||||
CHECK(assert_equal(actual, repeat(2, 1.0/0.0)));
|
||||
}
|
||||
|
||||
TEST ( NonlinearEquality, equals ) {
|
||||
string key1 = "x";
|
||||
Vector value1 = Vector_(2, 1.0, 2.0);
|
||||
Vector value2 = Vector_(2, 3.0, 4.0);
|
||||
VectorConfig feasible1, feasible2;
|
||||
feasible1.insert(key1, value1);
|
||||
feasible2.insert(key1, value2);
|
||||
|
||||
// create some constraints to compare
|
||||
shared_nle nle1(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
|
||||
shared_nle nle2(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
|
||||
shared_nle nle3(new NonlinearEquality<VectorConfig>(key1, feasible2, 2, *vector_compare));
|
||||
|
||||
// verify
|
||||
CHECK(nle1->equals(*nle2)); // basic equality = true
|
||||
CHECK(nle2->equals(*nle1)); // test symmetry of equals()
|
||||
CHECK(!nle1->equals(*nle3)); // test config
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue