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

18
gtsam/inference/FactorGraph.h
View File

@ -69,7 +69,7 @@ namespace gtsam {
/** Add a factor */ /** Add a factor */
template<class DERIVEDFACTOR> 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 */ /** push back many factors */
void push_back(const FactorGraph<FACTOR>& factors); void push_back(const FactorGraph<FACTOR>& factors);
@ -198,10 +198,7 @@ namespace gtsam {
FactorGraph<FACTOR>::FactorGraph(const FactorGraph<DERIVEDFACTOR>& factorGraph) { FactorGraph<FACTOR>::FactorGraph(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
factors_.reserve(factorGraph.size()); factors_.reserve(factorGraph.size());
BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) { BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) {
if(factor) this->push_back(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<class FACTOR>
template<typename ITERATOR> 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) { VectorValues steepestDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters, true); return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters, true);
} }
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) { VectorValues conjugateGradientDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, 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(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, 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 zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
@ -516,7 +516,7 @@ TEST(GaussianFactor, eliminateFrontals)
factors.push_back(factor4); factors.push_back(factor4);
// Create combined factor // 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 // Copies factors as they will be eliminated in place
JacobianFactor actualFactor_QR = combined; JacobianFactor actualFactor_QR = combined;

16
gtsam/slam/smallExample.cpp
View File

@ -163,7 +163,7 @@ namespace example {
else else
fg.add(ordering["l1"], 5*eye(2), ordering["x2"], -5*eye(2), Vector_(2, -1.0, 1.5), unit2); 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); Vector b1(2);
b1(0) = 1.0; b1(0) = 1.0;
b1(1) = -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 // create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_ // 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 Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1; Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0); 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)); constraintModel));
// construct the graph // construct the graph
@ -328,7 +328,7 @@ namespace example {
b1(0) = 1.0; b1(0) = 1.0;
b1(1) = -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_, 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 // create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_ // 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; Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10; Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0); 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)); constraintModel));
// construct the graph // construct the graph
@ -365,7 +365,7 @@ namespace example {
// unary factor 1 // unary factor 1
Matrix A = eye(2); Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0); 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 // constraint 1
Matrix A11(2, 2); Matrix A11(2, 2);
@ -383,7 +383,7 @@ namespace example {
Vector b1(2); Vector b1(2);
b1(0) = 1.0; b1(0) = 1.0;
b1(1) = 2.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)); constraintModel));
// constraint 2 // constraint 2
@ -402,7 +402,7 @@ namespace example {
Vector b2(2); Vector b2(2);
b2(0) = 3.0; b2(0) = 3.0;
b2(1) = 4.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)); constraintModel));
// construct the graph // construct the graph

2
tests/testGaussianFactorGraph.cpp
View File

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