diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index d881c2cd2..fea4f21d3 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -64,6 +64,18 @@ VectorConfig LinearFactorGraph::optimize(const Ordering& ordering) return newConfig; } +/* ************************************************************************* */ +boost::shared_ptr +LinearFactorGraph::eliminate_(const Ordering& ordering) { + return boost::shared_ptr(new GaussianBayesNet(eliminate(ordering))); +} + +/* ************************************************************************* */ +boost::shared_ptr +LinearFactorGraph::optimize_(const Ordering& ordering) { + return boost::shared_ptr(new VectorConfig(optimize(ordering))); +} + /* ************************************************************************* */ void LinearFactorGraph::combine(const LinearFactorGraph &lfg){ for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){ diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index 4976da2cb..232c600eb 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -83,6 +83,12 @@ namespace gtsam { */ VectorConfig optimize(const Ordering& ordering); + /** + * shared pointer versions for MATLAB + */ + boost::shared_ptr eliminate_(const Ordering&); + boost::shared_ptr optimize_(const Ordering&); + /** * static function that combines two factor graphs * @param const &lfg1 Linear factor graph diff --git a/cpp/gtsam.h b/cpp/gtsam.h index 3f3a872cb..a141b4665 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -48,13 +48,13 @@ class ConditionalGaussian { ConditionalGaussian(string key, Vector d, Matrix R, - Vector precisions); + Vector sigmas); ConditionalGaussian(string key, Vector d, Matrix R, string name1, Matrix S, - Vector precisions); + Vector sigmas); ConditionalGaussian(string key, Vector d, Matrix R, @@ -62,7 +62,7 @@ class ConditionalGaussian { Matrix S, string name2, Matrix T, - Vector precisions); + Vector sigmas); void print() const; Vector solve(const VectorConfig& x); void add(string key, Matrix S); @@ -91,8 +91,8 @@ class LinearFactorGraph { void print() const; bool equals(const LinearFactorGraph& lfgraph) const; - VectorConfig optimize(const Ordering& ordering); - void eliminate(const Ordering& ordering); + VectorConfig* optimize_(const Ordering& ordering); + GaussianBayesNet* eliminate_(const Ordering& ordering); pair matrix(const Ordering& ordering) const; Matrix sparse(const Ordering& ordering) const; }; diff --git a/matlab/createNoisyConfig.m b/matlab/createNoisyConfig.m index 92b2d917d..8da64f6da 100644 --- a/matlab/createNoisyConfig.m +++ b/matlab/createNoisyConfig.m @@ -3,7 +3,7 @@ function c = createNoisyConfig() v_x1 = [0.1; 0.1]; v_x2 = [1.4; 0.2]; v_l1 = [0.1; -1.1]; -c = FGConfig(); +c = VectorConfig(); c.insert('x1', v_x1); c.insert('x2', v_x2); c.insert('l1', v_l1); diff --git a/matlab/testLinearFactor.m b/matlab/testLinearFactor.m index 9f82721dd..106dbb4ef 100644 --- a/matlab/testLinearFactor.m +++ b/matlab/testLinearFactor.m @@ -26,7 +26,7 @@ Ax1 = [ % the RHS b2=[-1;1.5;2;-1]; -combined = LinearFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2); +combined = LinearFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, 1); % eliminate the combined factor % NOT WORKING @@ -48,7 +48,7 @@ S13 = [ +0.00,-8.94427 ]; d=[2.23607;-1.56525]; -expectedCG = ConditionalGaussian(d,R11,'l1',S12,'x1',S13); +expectedCG = ConditionalGaussian('x2',d,R11,'l1',S12,'x1',S13,[1 1]'); % the expected linear factor Bl1 = [ @@ -65,7 +65,7 @@ Bx1 = [ % the RHS b1= [0.0;0.894427]; -expectedLF = LinearFactor('l1', Bl1, 'x1', Bx1, b1); +expectedLF = LinearFactor('l1', Bl1, 'x1', Bx1, b1, 1); % check if the result matches % NOT WORKING diff --git a/matlab/test_time.m b/matlab/test_time.m index 47d8e7124..b7440124b 100644 --- a/matlab/test_time.m +++ b/matlab/test_time.m @@ -62,7 +62,7 @@ for steps=1:noRuns % eliminate with that ordering %time gt_sam ck=cputime; - BayesNet = linearFactorGraph.eliminate(ord); + BayesNet = linearFactorGraph.eliminate_(ord); time_gtsam=[time_gtsam,(cputime-ck)]; clear trajectory visibility linearFactorGraph measurements odometry;