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

Fix Matlab tests and add saveGraph method to GaussianBayesNet

release/4.3a0
Varun Agrawal 2021-03-10 15:58:12 -05:00
parent ad9e8536e7
commit faf004347b
9 changed files with 56 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/gtsam.i
View File

@ -1791,6 +1791,8 @@ virtual class GaussianBayesNet {
gtsam::KeySet keys() const;
bool exists(size_t idx) const;
void saveGraph(const string& s) const;
gtsam::GaussianConditional* front() const;
gtsam::GaussianConditional* back() const;
void push_back(gtsam::GaussianConditional* conditional);

25
gtsam/linear/GaussianBayesNet.cpp
View File

@ -12,15 +12,16 @@
/**
* @file GaussianBayesNet.cpp
* @brief Chordal Bayes Net, the result of eliminating a factor graph
* @author Frank Dellaert
* @author Frank Dellaert, Varun Agrawal
*/
#include <gtsam/base/timing.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/base/timing.h>
#include <boost/range/adaptor/reversed.hpp>
#include <fstream>
using namespace std;
using namespace gtsam;
@ -204,5 +205,23 @@ namespace gtsam {
}
/* ************************************************************************* */
void GaussianBayesNet::saveGraph(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::ofstream of(s.c_str());
of << "digraph G{\n";
for (auto conditional : boost::adaptors::reverse(*this)) {
typename GaussianConditional::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename GaussianConditional::Parents parents = conditional->parents();
for (Key p : parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
}
of << "}";
of.close();
}
/* ************************************************************************* */
} // namespace gtsam

10
gtsam/linear/GaussianBayesNet.h
View File

@ -177,6 +177,16 @@ namespace gtsam {
*/
VectorValues backSubstituteTranspose(const VectorValues& gx) const;
/**
* @brief Save the GaussianBayesNet as an image. Requires `dot` to be
* installed.
*
* @param s The name of the figure.
* @param keyFormatter Formatter to use for styling keys in the graph.
*/
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// @}
private:

4
matlab/+gtsam/plotBayesNet.m
View File

@ -5,4 +5,6 @@ function plotBayesNet(bayesNet)
bayesNet.saveGraph('/tmp/bayesNet.dot')
!dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot
dotImage=imread('/tmp/dotImage.png');
imshow(dotImage)
imshow(dotImage)
end

4
matlab/+gtsam/plotBayesTree.m
View File

@ -5,4 +5,6 @@ function plotBayesTree(bayesTree)
bayesTree.saveGraph('/tmp/bayesTree.dot')
!dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot
dotImage=imread('/tmp/dotImage.png');
imshow(dotImage)
imshow(dotImage)
end

2
matlab/gtsam_tests/testThinBayesTree.m
View File

@ -20,4 +20,4 @@
%% Run the tests
import gtsam.*
bayesTree = thinBayesTree(3,2);
EQUALITY('7 = bayesTree.size', 7, bayesTree.size);
EQUALITY('7 = bayesTree.size', 4, bayesTree.size);

10
matlab/gtsam_tests/testTriangulation.m
View File

@ -44,15 +44,15 @@ optimize = true;
rank_tol = 1e-9;
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);
%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements = Point2Vector;
measurements.push_back(z1.retract([0.1;0.5]));
measurements.push_back(z2.retract([-0.2;0.3]));
measurements.push_back(z1 + [0.1;0.5]);
measurements.push_back(z2 + [-0.2;0.3]);
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2));
CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-2);
%% two Poses with Bundler Calibration
bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480);
@ -67,4 +67,4 @@ measurements.push_back(z1);
measurements.push_back(z2);
triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);

3
matlab/gtsam_tests/thinBayesTree.m
View File

@ -1,5 +1,6 @@
function bayesTree = thinBayesTree(depth, width)
import gtsam.*
bayesNet = thinTreeBayesNet(depth, width);
bayesTree = GaussianBayesTree(bayesNet);
fg = GaussianFactorGraph(bayesNet);
bayesTree = fg.eliminateMultifrontal();
end

13
matlab/gtsam_tests/thinTreeBayesNet.m
View File

@ -6,8 +6,9 @@ bayesNet = GaussianBayesNet;
tree = thinTree(depth,width);
% Add root to the Bayes net
gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1));
bayesNet.push_front(gc);
model = noiseModel.Isotropic.Sigma(1, 3*rand(1));
gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), model);
bayesNet.push_back(gc);
n=tree.getNumberOfElements();
for i=2:n
@ -17,13 +18,15 @@ for i=2:n
% Create and link the corresponding GaussianConditionals
if tree.getW == 1 || di == 2
% Creation of single-parent GaussianConditional
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1));
model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), model);
elseif tree.getW == 2 || di == 3
% GaussianConditionalj associated with the second parent
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1));
model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), model);
end
% Add conditional to the Bayes net
bayesNet.push_front(gc);
bayesNet.push_back(gc);
end
end