Added ffg.DynamicNetwork #36

improved error output #30

cpp-factor-graph/matlab/ffg/+ffg/+tests/testDynamicNetwork.m

@@ -0,0 +1,82 @@
+function test_suite = testDynamicNetwork
+    initTestSuite;
+
+
+function testAdjacencyTemporal
+
+    a = ffg.EvidenceNode;
+    b = ffg.EvidenceNode;
+    c = ffg.EvidenceNode;
+    node = ffg.AddNode;
+
+    nwk = ffg.DynamicNetwork;
+
+    nwk.addEdge(a, node);
+    nwk.addEdge(b, node);
+    nwk.addEdge(node, c);
+
+    nwk.addTemporalEdge(a, b);
+
+    EXPECTED_MATRIX = zeros(4,4);
+    EXPECTED_MATRIX(1,2) = 1;
+
+    assertElementsAlmostEqual(nwk.adjacencyMatrixTemporal(), EXPECTED_MATRIX);
+
+
+function testOverallKalmanFilter
+% testing whether the dynamic network works in general
+    xin = ffg.EvidenceNode;
+    xout = ffg.EvidenceNode;
+    n = ffg.EvidenceNode;
+    y = ffg.EvidenceNode;
+    e = ffg.EqualityNode;
+    a = ffg.AddNode;
+    u = ffg.EvidenceNode;
+    b = ffg.AddNode;
+
+    nwk = ffg.DynamicNetwork;
+
+    nwk.addEdge(xin, e);
+    nwk.addEdge(e, b);
+    nwk.addEdge(u, b);
+    nwk.addEdge(b, xout);
+    nwk.addEdge(e, a);
+    nwk.addEdge(a, y);
+    nwk.addEdge(n, a);
+
+    nwk.addTemporalEdge(xout, xin);
+
+    schedule = {xin, e; ...
+                n, a; ...
+                y, a; ...
+                a, e; ...
+                e, b; ...
+                u, b; ...
+                b, xout};
+
+    nwk.setSchedule(schedule);
+
+    sd = 10.0;
+    sd2 = sd*sd;
+    u_const = 1.0;
+
+    xout.receive(ffg.gaussMessage(1+randn()*sd, sd2, 'VARIANCE'));
+    n.receive(ffg.gaussMessage(0, sd2, 'VARIANCE'));
+    u.receive(ffg.gaussMessage(u_const, 0,'VARIANCE'));
+    xin.receive(ffg.gaussMessage(1+randn()*sd, sd2, 'VARIANCE'));
+
+    ffg.drawNetwork(nwk, 3);
+
+    NUM_ITERATIONS = 1000;
+    results = struct('id', {}, 'message', {});
+    for i = 1:NUM_ITERATIONS
+        results = [results struct('id', y.id, 'message', ffg.gaussMessage(i+randn()*sd, 0, 'VARIANCE'))];
+    end
+    nwk.makeStep(results, 1);
+
+    EXPECTED_MEAN = NUM_ITERATIONS;
+    EXPECTED_VAR = 1e-1;
+
+    assertElementsAlmostEqual(EXPECTED_MEAN, xout.evidence().mean, 'absolute', 2);
+    assertElementsAlmostEqual(EXPECTED_VAR, xout.evidence().var, 'absolute', 1e-1);
+

cpp-factor-graph/matlab/ffg/+ffg/DynamicNetwork.m

@@ -0,0 +1,42 @@
+classdef DynamicNetwork < ffg.Network
+    %DYNAMICNETWORK network with repeated structure
+    %   The dynamic network consists of interconnected time slices, each
+    %   represented by a smaller static network
+
+    methods
+        function this = DynamicNetwork()
+            this = this@ffg.Network('DynamicNetwork');
+        end
+
+        function addTemporalEdge(this, src, dst)
+            % (ffg) add a temporal edge between two nodes of consequent time slices
+            % INPUTS:
+            %   src - source node at the step (n-1)
+            %   dst - destination node at the step (n)
+            mexfactorgraph('addTemporalEdge', this.type_name, this.cpp_handle, src.cpp_handle, dst.cpp_handle);
+        end
+
+        function result = adjacencyMatrixTemporal(this)
+            % (ffg) get an adjacency matrix of the network, for edges between different time slices
+            result = mexfactorgraph('adjacencyMatrixTemporal', this.type_name, this.cpp_handle);
+        end
+
+        function makeStep(this, data, count)
+            % (ffg) make several steps with data (or one if no data)
+            % INPUTS:
+            %   data - array of structs struct('id',node_id, 'message', msg),
+            %   where 'id' contains id of the EvidenceNode that will
+            %   receive the message contained in 'message'. the size of the
+            %   array should be <number_of_steps> * count
+            %   count - the number of messages per step 
+            if nargin == 3
+                mexfactorgraph('step', this.type_name, this.cpp_handle, data, count);
+            else
+                mexfactorgraph('step', this.type_name, this.cpp_handle);
+            end
+        end
+
+    end
+
+end
+

cpp-factor-graph/matlab/ffg/+ffg/Network.m

@@ -8,9 +8,11 @@
 
 
     methods
-        function this = Network()
-            this = this@ffg.CppObject('Network');
-            % NOTE: this can't be codegened?
+        function this = Network(type)
+            if nargin == 0
+                type = 'Network';
+            end
+            this = this@ffg.CppObject(type);
             this.node_list = containers.Map('KeyType', 'int32', 'ValueType', 'any');
         end
 

cpp-factor-graph/matlab/ffg/+ffg/drawNetwork.m

@@ -1,13 +1,15 @@
-function drawNetwork(network)
+function drawNetwork(network, numrep)
 %DRAWNETWORK draw the network graph
-%   network - ffg.Network do draw 
 % the following notations are used:
 %  "=" EqualityNode
 %  "+" AddNode
 %  "*" MultiplicationNode
 %  "X" EvidenceNode
 %  "*=" EquMultNode
 %  "*" EstimateMultiplicationNode
+% INPUTS:
+%   network - ffg.Network do draw 
+%   numrep - for dynamic networks only, the number of times to repeat
 
     nodes = network.nodes();
     labels = {};
@@ -28,8 +30,14 @@ function drawNetwork(network)
             labels{i} = nodes{i}.type_name;
         end
     end
-
-    draw_layout(network.adjacencyMatrix(),  labels);
-    %draw_layout_dbn(network.adjacencyMatrix(),  zeros(length(nodes), length(nodes)), 0, 2, labels);
+
+    if strcmp(class(network), 'ffg.DynamicNetwork') && nargin == 2
+        draw_layout_dbn(network.adjacencyMatrix(),  network.adjacencyMatrixTemporal(), 0, numrep, labels);
+    elseif strcmp(class(network), 'ffg.DynamicNetwork')
+        draw_layout_dbn(network.adjacencyMatrix(),  network.adjacencyMatrixTemporal(), 0, 2, labels);
+    else
+        draw_layout(network.adjacencyMatrix(),  labels);
+    end
+    %
 end
 

cpp-factor-graph/matlab/ffg/examples/drawExample.m

@@ -0,0 +1,25 @@
+function drawExample
+%DRAWEXAMPLE drawing network of a kalman filter
+
+    xin = ffg.EvidenceNode;
+    xout = ffg.EvidenceNode;
+    n = ffg.EvidenceNode;
+    y = ffg.EvidenceNode;
+    e = ffg.EqualityNode;
+    a = ffg.AddNode;
+    u = ffg.EvidenceNode;
+    b = ffg.AddNode;
+
+    nwk = ffg.Network;
+    nwk.addEdge(xin, e);
+    nwk.addEdge(e, b);
+    nwk.addEdge(u, b);
+    nwk.addEdge(b, xout);
+    nwk.addEdge(e, a);
+    nwk.addEdge(a, y);
+    nwk.addEdge(n, a);
+
+
+    ffg.drawNetwork(nwk);
+end
+

cpp-factor-graph/matlab/ffg/examples/dynamicNetworkExample.m

@@ -0,0 +1,56 @@
+function dynamicNetworkExample
+%DYNAMICNETWORKEXAMPLE example of using DynamicNetwork for filtering
+
+    xin = ffg.EvidenceNode;
+    xout = ffg.EvidenceNode;
+    n = ffg.EvidenceNode;
+    y = ffg.EvidenceNode;
+    e = ffg.EqualityNode;
+    a = ffg.AddNode;
+    u = ffg.EvidenceNode;
+    b = ffg.AddNode;
+
+    nwk = ffg.DynamicNetwork;
+
+    nwk.addEdge(xin, e);
+    nwk.addEdge(e, b);
+    nwk.addEdge(u, b);
+    nwk.addEdge(b, xout);
+    nwk.addEdge(e, a);
+    nwk.addEdge(a, y);
+    nwk.addEdge(n, a);
+
+    nwk.addTemporalEdge(xout, xin);
+
+    schedule = {xin, e; ...
+                n, a; ...
+                y, a; ...
+                a, e; ...
+                e, b; ...
+                u, b; ...
+                b, xout};
+
+    nwk.setSchedule(schedule);
+
+    sd = 10.0;
+    sd2 = sd*sd;
+    u_const = 1.0;
+
+    xout.receive(ffg.gaussMessage(1+randn()*sd, sd2, 'VARIANCE'));
+    n.receive(ffg.gaussMessage(0, sd2, 'VARIANCE'));
+    u.receive(ffg.gaussMessage(u_const, 0,'VARIANCE'));
+    xin.receive(ffg.gaussMessage(1+randn()*sd, sd2, 'VARIANCE'));
+
+    ffg.drawNetwork(nwk, 3);
+
+    results = struct('id', {}, 'message', {});
+    for i = 1:1000
+        results = [results struct('id', y.id, 'message', ffg.gaussMessage(i+randn()*sd, 0, 'VARIANCE'))];
+    end
+    nwk.makeStep(results, 1);
+
+    xout.evidence()
+
+
+end
+

cpp-factor-graph/matlab/ffg/examples/kalmanExample.m

@@ -41,9 +41,7 @@
     % for gaussians only:
     % 'mean' - 1xN vector
     % 'var' - NxN matrix
-    msg = ffg.gaussMessage(1+randn()*sd, sd2, 'VARIANCE');
-
-    xout.propagate(msg);
+    xout.propagate(ffg.gaussMessage(1+randn()*sd, sd2, 'VARIANCE'));
     n.propagate(ffg.gaussMessage(0, sd2, 'VARIANCE'));
     u.propagate(ffg.gaussMessage(u_const, 0,'VARIANCE'));
 
@@ -53,6 +51,7 @@
 
     result = zeros(N_ITERATIONS, 2);    
 
+    msg = ffg.gaussMessage(1+randn()*sd, sd2, 'VARIANCE');
     for i = 1:N_ITERATIONS
         xin.propagate(msg);
         samples(i,:) = i+randn()*sd;

cpp-factor-graph/matlab/ffg/graphlayout/draw_layout.m

@@ -78,7 +78,7 @@
     dy1 = sign.*wd(i,2).*sin(alpha);   dx1 = sign.*wd(i,1).*cos(alpha);
     dy2 = sign.*wd(k,2).*sin(alpha);   dx2 = sign.*wd(k,1).*cos(alpha);    
     if adj(k,i)==0, % if directed edge
-      arrow([x(i)+dx1 y(i)+dy1],[x(k)-dx2 y(k)-dy2]);
+      arrow('Start', [x(i)+dx1 y(i)+dy1],'Stop',[x(k)-dx2 y(k)-dy2], 'Length', 10);
     else	   
       line([x(i)+dx1 x(k)-dx2],[y(i)+dy1 y(k)-dy2],'color','k');
       adj(k,i)=-1; % Prevent drawing lines twice

cpp-factor-graph/matlab/ffg/graphlayout/textoval.m

@@ -66,8 +66,8 @@
 function [ptc, wx, wy] = draw_oval(tx, x, y)
 % Draws an oval box around a tex object
       sz = get(tx,'Extent');
-      wy = sz(4);
-      wx = max(2/3*sz(3), wy); 
+      wy = 0.5 * sz(4);
+      wx = 0.5 * max(2/3*sz(3), wy); 
       ptc = ellipse(x, y, wx, wy);
       set(ptc, 'FaceColor','w');
 

cpp-factor-graph/matlab/ffg/src/convert.h

@@ -13,6 +13,9 @@
 #include <factorgraph.h>
 
 // TODO: make a template-based type caster
+static const char *MEX_ID = "id";
+static const char *MEX_MESSAGE = "message";
+
 static const char *MEX_FROM = "from";
 static const char *MEX_TYPE = "type";
 static const char *MEX_CONN = "connection";
@@ -183,34 +186,55 @@ inline mxArray *messagesToStructArray(const MessageBox &msgs)
 /**
  * @brief get the adjacency matrix as a matlab 2D array for the network
  */
-inline mxArray *networkAdjacencyMatrix(const Network &nwk)
+inline mxArray *networkAdjacencyMatrix(const Network::NodeList &nodes, const Network::AdjList &adjList)
 {
-    mxArray *result = mxCreateDoubleMatrix(nwk.nodes().size(), nwk.nodes().size(), mxREAL);
-    if (nwk.nodes().empty())
+    mxArray *result = mxCreateDoubleMatrix(nodes.size(), nodes.size(), mxREAL);
+    if (nodes.empty())
         return result;
+
     double *data = mxGetPr(result);
     // getting the smallest id in the network
-    int minId = nwk.nodes().begin()->first;
-    for (Network::AdjListIt it = nwk.adjList().begin(); it != nwk.adjList().end(); ++it)
-        data[(it->second - minId) * nwk.nodes().size() + (it->first - minId)] = 1.0;
+    int minId = nodes.begin()->first;
+    for (Network::AdjListIt it = adjList.begin(); it != adjList.end(); ++it)
+        data[(it->second - minId) * nodes.size() + (it->first - minId)] = 1.0;
     return result;
 }
 
 
 /**
- * @brief networks
+ * @brief convert nodes to the matlab representation
  */
-inline mxArray *networkNodes(const Network &nwk)
+inline mxArray *networkNodes(const Network::NodeList &nodes)
 {
-    mxArray *result = mxCreateDoubleMatrix(1, nwk.nodes().size(), mxREAL);
+    mxArray *result = mxCreateDoubleMatrix(1, nodes.size(), mxREAL);
     double *data = mxGetPr(result);
     int i = 0;
-    for (std::map<int, FactorNode*>::const_iterator it = nwk.nodes().begin(); it != nwk.nodes().end(); ++it)
+    for (std::map<int, FactorNode*>::const_iterator it = nodes.begin(); it != nodes.end(); ++it)
         data[i++] = it->first;
     return result;
 }
 
 
+inline std::vector<MessageBox> createMessageBoxArray(const mxArray *mx_data, const mxArray *mx_step)
+{
+    std::vector<MessageBox> result;
+    size_t totalCount = mxGetN(mx_data);
+    size_t step = arrayToInt(mx_step);
+    // assert(step != 0);
+
+    for (size_t i = 0; i < totalCount; i += step)
+    {
+        MessageBox box;
+        for (size_t j = 0; j < step; j++)
+        {
+            int id = arrayToInt(mxGetField(mx_data, i + j, MEX_ID));
+            GaussianMessage msg = createGaussianMessage(mxGetField(mx_data, i + j, MEX_MESSAGE));
+            box.insert(std::make_pair(id, msg));
+        }
+        result.push_back(box);
+    }
+    return result;
+}
 
 
 #endif // CONVERT_H