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

testing new alphaFactor

release/4.3a0
Frank Dellaert 2009-12-11 18:03:43 +00:00
parent f91a1f0192
commit a66f08a5e0
3 changed files with 41 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cpp/GaussianFactor.cpp
View File

@ -126,13 +126,20 @@ bool GaussianFactor::equals(const Factor<VectorConfig>& f, double tol) const {
}
/* ************************************************************************* */
Vector GaussianFactor::error_vector(const VectorConfig& c) const {
Vector GaussianFactor::unweighted_error(const VectorConfig& c) const {
Vector e = -b_;
if (empty()) return e;
string j; Matrix Aj;
FOREACH_PAIR(j, Aj, As_)
e += Vector(Aj * c[j]);
return ediv(e,sigmas_);
return e;
}
/* ************************************************************************* */
Vector GaussianFactor::error_vector(const VectorConfig& c) const {
Vector e = -b_;
if (empty()) return e;
return ediv(unweighted_error(c),sigmas_);
}
/* ************************************************************************* */
@ -364,7 +371,8 @@ GaussianFactor::eliminate(const string& key) const
}
/* ************************************************************************* */
void GaussianFactor::addGradientContribution(const VectorConfig& x, VectorConfig& g) const {
void GaussianFactor::addGradientContribution(const VectorConfig& x,
VectorConfig& g) const {
// calculate the value of the factor
Vector e = GaussianFactor::error_vector(x);
Vector et = trans(e); // transpose
@ -385,13 +393,18 @@ void GaussianFactor::addGradientContribution(const VectorConfig& x, VectorConfig
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const VectorConfig& x,
const VectorConfig& d) const {
// Calculate A matrix
size_t m = b_.size();
Vector A = zero(m); Vector b = b_;
Vector A = zero(m);
string j; Matrix Aj;
FOREACH_PAIR(j, Aj, As_) {
FOREACH_PAIR(j, Aj, As_)
A += Aj * d[j];
b -= Aj * x[j];
}
// calculate the value of the factor for RHS
Vector b = - unweighted_error(x);
// construct factor
shared_ptr factor(new GaussianFactor("alpha",Matrix_(A),b,sigmas_));
return factor;
}

1
cpp/GaussianFactor.h
View File

@ -117,6 +117,7 @@ public:
// Implementing Factor virtual functions
Vector unweighted_error(const VectorConfig& c) const; /** (A*x-b) */
Vector error_vector(const VectorConfig& c) const; /** (A*x-b)/sigma */
double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
std::size_t size() const { return As_.size();}

20
cpp/testGaussianFactor.cpp
View File

@ -636,6 +636,26 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
CHECK(assert_equal(expectedLF,actualLF,1e-5));
}
/* ************************************************************************* */
TEST( GaussianFactor, alphaFactor )
{
GaussianFactorGraph fg = createGaussianFactorGraph();
// get alphafactor for first factor in fg at zero, in gradient direction
VectorConfig x = createZeroDelta();
VectorConfig d = fg.gradient(x);
GaussianFactor::shared_ptr factor = fg[0];
GaussianFactor::shared_ptr actual = factor->alphaFactor(x,d);
// calculate expected
Matrix A = Matrix_(2,1,30.0,5.0);
Vector b = Vector_(2,-0.1,-0.1);
Vector sigmas = Vector_(2,0.1,0.1);
GaussianFactor expected("alpha",A,b,sigmas);
CHECK(assert_equal(expected,*actual));
}
/* ************************************************************************* */
TEST ( GaussianFactor, constraint_eliminate1 )
{