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

Fixed NonlinearFactor2 equals and added some unit tests for equals

release/4.3a0
Frank Dellaert 2009-10-24 20:01:47 +00:00
parent 06a7898da2
commit 3792c79706
5 changed files with 73 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
.cproject
View File

@ -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,6 @@
</target>
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -322,6 +323,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 +339,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 +346,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 +354,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 +361,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 +377,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 +385,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,6 +392,7 @@
</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>
@ -397,6 +400,7 @@
</target>
<target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -404,6 +408,7 @@
</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 +416,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,6 +423,7 @@
</target>
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -426,7 +431,6 @@
</target>
<target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -434,7 +438,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>
@ -442,15 +445,21 @@
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<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>
@ -458,7 +467,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>
@ -466,7 +474,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>

24
cpp/NonlinearFactor.cpp
View File

@ -19,7 +19,7 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z,
Vector (*h)(const Vector&),
const string& key1,
Matrix (*H)(const Vector&))
: NonlinearFactor<VectorConfig>(z, sigma), h_(h), key1_(key1), H_(H)
: NonlinearFactor<VectorConfig>(z, sigma), h_(h), key_(key1), H_(H)
{
keys_.push_front(key1);
}
@ -27,13 +27,16 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z,
/* ************************************************************************* */
void NonlinearFactor1::print(const string& s) const {
cout << "NonLinearFactor1 " << s << endl;
NonlinearFactor<VectorConfig>::print(s);
cout << "h : " << (void*)h_ << endl;
cout << "key: " << key_ << endl;
cout << "H : " << (void*)H_ << endl;
NonlinearFactor<VectorConfig>::print("parent");
}
/* ************************************************************************* */
LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const {
// get argument 1 from config
Vector x1 = c[key1_];
Vector x1 = c[key_];
// Jacobian A = H(x1)/sigma
Matrix A = H_(x1) / sigma_;
@ -41,7 +44,7 @@ LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) cons
// Right-hand-side b = error(c) = (z - h(x1))/sigma
Vector b = (z_ - h_(x1)) / sigma_;
LinearFactor::shared_ptr p(new LinearFactor(key1_, A, b));
LinearFactor::shared_ptr p(new LinearFactor(key_, A, b));
return p;
}
@ -53,7 +56,7 @@ bool NonlinearFactor1::equals(const Factor<VectorConfig>& f, double tol) const {
if (p == NULL) return false;
return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_)
&& (key1_== p->key1_)
&& (key_ == p->key_)
&& (H_ == p->H_);
}
@ -75,7 +78,12 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z,
/* ************************************************************************* */
void NonlinearFactor2::print(const string& s) const {
cout << "NonLinearFactor2 " << s << endl;
NonlinearFactor<VectorConfig>::print(s);
cout << "h : " << (void*)h_ << endl;
cout << "key1: " << key1_ << endl;
cout << "H2 : " << (void*)H2_ << endl;
cout << "key2: " << key2_ << endl;
cout << "H1 : " << (void*)H1_ << endl;
NonlinearFactor<VectorConfig>::print("parent");
}
/* ************************************************************************* */
@ -102,9 +110,9 @@ bool NonlinearFactor2::equals(const Factor<VectorConfig>& f, double tol) const {
return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_)
&& (key1_ == p->key1_)
&& (H2_ == p->H1_)
&& (H1_ == p->H1_)
&& (key2_ == p->key2_)
&& (H1_ == p->H2_);
&& (H2_ == p->H2_);
}
/* ************************************************************************* */

10
cpp/NonlinearFactor.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
}
/** Check if two NonlinearFactor objects are equal */
bool equals(const Factor<Config>& f, double tol) const {
bool equals(const Factor<Config>& f, double tol=1e-9) const {
const NonlinearFactor<Config>* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
if (p == NULL) return false;
return equal_with_abs_tol(z_,p->z_,tol) && fabs(sigma_ - p->sigma_)<=tol;
@ -123,7 +123,7 @@ namespace gtsam {
class NonlinearFactor1 : public NonlinearFactor<VectorConfig> {
private:
std::string key1_;
std::string key_;
public:
@ -144,7 +144,7 @@ namespace gtsam {
/** error function */
inline Vector error_vector(const VectorConfig& c) const {
return z_ - h_(c[key1_]);
return z_ - h_(c[key_]);
}
/** linearize a non-linearFactor1 to get a linearFactor1 */
@ -172,8 +172,8 @@ namespace gtsam {
Matrix (*H2_)(const Vector&, const Vector&);
/** Constructor */
NonlinearFactor2(const Vector& z, // the measurement
const double sigma, // the variance
NonlinearFactor2(const Vector& z, // the measurement
const double sigma, // the variance
Vector (*h)(const Vector&, const Vector&), // the measurement function
const std::string& key1, // key of the first variable
Matrix (*H1)(const Vector&, const Vector&), // derivative of h in first variable

3
cpp/smallExample.cpp
View File

@ -6,9 +6,6 @@
* @author Frank dellaert
*/
/*STL/C++*/
#include <iostream>
#include <string>

34
cpp/testNonlinearFactor.cpp
View File

@ -13,12 +13,46 @@
#include "Matrix.h"
#include "smallExample.h"
#include "Simulated2DMeasurement.h"
using namespace std;
using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
/* ************************************************************************* */
TEST( NonLinearFactor, equals )
{
double sigma = 1.0;
// create two nonlinear2 factors
Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
Simulated2DMeasurement f0(z3, sigma, "x1", "l1");
// measurement between x2 and l1
Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
Simulated2DMeasurement f1(z4, sigma, "x2", "l1");
CHECK(assert_equal(f0,f0));
CHECK(f0.equals(f0));
CHECK(!f0.equals(f1));
CHECK(!f1.equals(f0));
}
/* ************************************************************************* */
TEST( NonLinearFactor, equals2 )
{
// create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
// get two factors
shared_nlf f0 = fg[0], f1 = fg[1];
CHECK(f0->equals(*f0));
CHECK(!f0->equals(*f1));
CHECK(!f1->equals(*f0));
}
/* ************************************************************************* */
TEST( NonLinearFactor, NonlinearFactor )
{