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

Removed incorrect dynamic_pointer_cast that caused a compile error for non-virtual derived factors.

release/4.3a0
Richard Roberts 2011-02-18 22:10:21 +00:00
parent 299ce59c61
commit 1c27b1021f
5 changed files with 17 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/inference/FactorGraph.h
View File

@ -69,7 +69,7 @@ namespace gtsam {
/** Add a factor */
template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor);
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) { factors_.push_back(sharedFactor(factor)); }
/** push back many factors */
void push_back(const FactorGraph<FACTOR>& factors);
@ -198,10 +198,7 @@ namespace gtsam {
FactorGraph<FACTOR>::FactorGraph(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
factors_.reserve(factorGraph.size());
BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) {
if(factor)
this->push_back(factor);
else
this->push_back(sharedFactor());
}
}
@ -215,17 +212,6 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
inline void FactorGraph<FACTOR>::push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
#ifndef NDEBUG
factors_.push_back(boost::dynamic_pointer_cast<FACTOR>(factor)); // add the actual factor
#else
factors_.push_back(boost::static_pointer_cast<FACTOR>(factor));
#endif
}
/* ************************************************************************* */
template<class FACTOR>
template<typename ITERATOR>

8
gtsam/linear/iterative.cpp
View File

@ -56,12 +56,12 @@ namespace gtsam {
}
/* ************************************************************************* */
VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters, true);
VectorValues steepestDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters, true);
}
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters);
VectorValues conjugateGradientDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters);
}
/* ************************************************************************* */

4
gtsam/linear/tests/testGaussianFactor.cpp
View File

@ -174,7 +174,7 @@ TEST(GaussianFactor, Combine2)
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
JacobianFactor actual = *JacobianFactor::Combine(gfg, VariableSlots(gfg));
JacobianFactor actual = *JacobianFactor::Combine(*gfg.dynamicCastFactors<FactorGraph<JacobianFactor> >(), VariableSlots(gfg));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
@ -516,7 +516,7 @@ TEST(GaussianFactor, eliminateFrontals)
factors.push_back(factor4);
// Create combined factor
JacobianFactor combined(*JacobianFactor::Combine(factors, VariableSlots(factors)));
JacobianFactor combined(*JacobianFactor::Combine(*factors.dynamicCastFactors<FactorGraph<JacobianFactor> >(), VariableSlots(factors)));
// Copies factors as they will be eliminated in place
JacobianFactor actualFactor_QR = combined;

16
gtsam/slam/smallExample.cpp
View File

@ -163,7 +163,7 @@ namespace example {
else
fg.add(ordering["l1"], 5*eye(2), ordering["x2"], -5*eye(2), Vector_(2, -1.0, 1.5), unit2);
return fg;
return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
}
/* ************************************************************************* */
@ -290,7 +290,7 @@ namespace example {
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
@ -299,7 +299,7 @@ namespace example {
Matrix Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0);
GaussianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
@ -328,7 +328,7 @@ namespace example {
b1(0) = 1.0;
b1(1) = -1.0;
//GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
@ -341,7 +341,7 @@ namespace example {
Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
GaussianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
@ -365,7 +365,7 @@ namespace example {
// unary factor 1
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
GaussianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));
JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));
// constraint 1
Matrix A11(2, 2);
@ -383,7 +383,7 @@ namespace example {
Vector b1(2);
b1(0) = 1.0;
b1(1) = 2.0;
GaussianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
constraintModel));
// constraint 2
@ -402,7 +402,7 @@ namespace example {
Vector b2(2);
b2(0) = 3.0;
b2(1) = 4.0;
GaussianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
constraintModel));
// construct the graph

2
tests/testGaussianFactorGraph.cpp
View File

@ -691,7 +691,7 @@ TEST( GaussianFactorGraph, multiplication )
// create an ordering
Ordering ord; ord += "x2","l1","x1";
GaussianFactorGraph A = createGaussianFactorGraph(ord);
FactorGraph<JacobianFactor> A = createGaussianFactorGraph(ord);
VectorValues x = createCorrectDelta(ord);
Errors actual = A * x;
Errors expected;