schema/state/StateParams5.schema

+value:
+  _type: Vector5d
+  _mandatory: true
+  _doc: A vector containing the state values.
+
+prior:
+  mode:
+    _type: string
+    _mandatory: true
+    _options: ["fix", "factor", "initial_guess"]
+    _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+  factor_std:
+    _type: Vector5d
+    _mandatory: $mode == 'factor'
+    _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.

schema/state/StateParams6.schema

+value:
+  _type: Vector6d
+  _mandatory: true
+  _doc: A vector containing the state values.
+
+prior:
+  mode:
+    _type: string
+    _mandatory: true
+    _options: ["fix", "factor", "initial_guess"]
+    _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+  factor_std:
+    _type: Vector6d
+    _mandatory: $mode == 'factor'
+    _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.

schema/state/StateParams7.schema

+value:
+  _type: Vector7d
+  _mandatory: true
+  _doc: A vector containing the state values.
+
+prior:
+  mode:
+    _type: string
+    _mandatory: true
+    _options: ["fix", "factor", "initial_guess"]
+    _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+  factor_std:
+    _type: Vector7d
+    _mandatory: $mode == 'factor'
+    _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.

schema/state/StateParams8.schema

+value:
+  _type: Vector8d
+  _mandatory: true
+  _doc: A vector containing the state values.
+
+prior:
+  mode:
+    _type: string
+    _mandatory: true
+    _options: ["fix", "factor", "initial_guess"]
+    _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+  factor_std:
+    _type: Vector8d
+    _mandatory: $mode == 'factor'
+    _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.

schema/state/StateParams9.schema

+value:
+  _type: Vector9d
+  _mandatory: true
+  _doc: A vector containing the state values.
+
+prior:
+  mode:
+    _type: string
+    _mandatory: true
+    _options: ["fix", "factor", "initial_guess"]
+    _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+  factor_std:
+    _type: Vector9d
+    _mandatory: $mode == 'factor'
+    _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.

schema/state/StateV2d.schema

+value:
+  _type: Vector2d
+  _mandatory: true
+  _doc: A vector containing the velocity (x, y) [m/s].
+
+prior:
+  mode:
+    _type: string
+    _mandatory: true
+    _options: ["fix", "factor", "initial_guess"]
+    _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+  factor_std:
+    _type: Vector2d
+    _mandatory: $mode == 'factor'
+    _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m/s].

schema/state/StateV3d.schema

+value:
+  _type: Vector3d
+  _mandatory: true
+  _doc: A vector containing the velocity (x, y, z) [m/s].
+
+prior:
+  mode:
+    _type: string
+    _mandatory: true
+    _options: ["fix", "factor", "initial_guess"]
+    _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+  factor_std:
+    _type: Vector3d
+    _mandatory: $mode == 'factor'
+    _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m/s].

schema/tree_manager/TreeManagerBase.schema

+type:
+  _mandatory: true
+  _type: string
+  _doc: Type of the TreeManager. To keep all frames, use "none".
\ No newline at end of file

schema/tree_manager/TreeManagerSlidingWindow.schema

+follow: TreeManagerBase.schema
+
+n_frames:
+  _mandatory: true
+  _type: unsigned int 
+  _doc: Total number of frames of the sliding window.
+
+n_fix_first_frames:
+  _mandatory: true
+  _type: unsigned int 
+  _doc: Amount of frames fixed at the begining of the sliding window. 
+
+viral_remove_parent_without_children:
+  _mandatory: true
+  _type: bool 
+  _doc: If the other wolf nodes (landmarks) have to be removed when removing frames. Otherwise, they will remain alive but unconstrained.

schema/tree_manager/TreeManagerSlidingWindowDualRate.schema

+follow: TreeManagerSlidingWindow.schema
+
+n_frames_recent:
+  _mandatory: true
+  _type: unsigned int
+  _doc: Amount of frames kept in the recent part of the sliding window.
+
+rate_old_frames:
+  _mandatory: true
+  _type: unsigned int 
+  _doc: Rate of keyframes that are kept from the recent part to the sparse part of the sliding window. One of each 'rate_old_frames' will be kept.
\ No newline at end of file

scilab/corner_detector.sce

-//info about 2d homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
-
-// clear all 
-xdel(winsid());
-clear;
-
-//scan params:
-Ns = 720; //scan rays
-aperture = %pi; //scan aperture [rad]
-azimuth_step = aperture/Ns;
-
-//User Tunning params
-Nw = 8; //window size
-theta_th = %pi/8;
-theta_max = 0.3;
-K = 3; //How many std_dev are tolerated to count that a point is supporting a line
-r_stdev = 0.01; //ranging std dev
-max_beam_dist = 5; //max distance in amount of beams to consider concatenation of two lines
-max_dist = 0.2; //max distance to a corner from the ends of both lines to take it
-range_jump_th = 0.5; //threshold of distance to detect jumps in ranges
-max_points_line = 1000; //max amount of beams of a line
-
-//init
-points = [];
-result_lines = [];
-line_indexes = [];
-corners = [];
-corners_jumps = [];
-jumps = [];
-
-//scan ranges
-ranges = read(fullpath('scan.txt'),-1,Ns);
-//ranges = [];
-
-//invent a set of points + noise
-//points = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24  25 26  27 28  29 30  31 32  33  34  35  36  37 38  39  40  41  42  43;
-//          7 6 5 4 3 2 1 2 3 4  5  6  7  8  9  10 9  8  7  6  5  4  3  3.5 4  4.5 5  5.5 6  6.5 7  7.5 7.4 7.3 7.2 7.1 7  6.9 6.8 6.7 6.6 6.5 6.4];
-for i=1:Ns
-   points = [points [ranges(i)*cos(aperture/2 - azimuth_step*i); ranges(i)*sin(aperture/2 - azimuth_step*i)]];
-   // Store range jumps
-   if i>1 & abs(ranges(i)-ranges(i-1)) > range_jump_th then
-       jumps = [jumps i];
-   end
-end
-
-points = points + rand(points,"normal")*r_stdev;
-
-//Main loop. Runs over a sliding window of Nw points
-i_jump = 1;
-for i = Nw:size(points,2)
-    
-    //set the window indexes
-    i_from = i-Nw+1;
-    i_to = i;
-    points_w = points(:,i_from:i_to);
-    
-    //update the jump to be checked
-    while i_jump < size(jumps,2) & i_from > jumps(i_jump)
-        i_jump = i_jump+1;
-    end
-    
-    //check if there is a jump inside the window (if it is the case, do not fit a line)
-    if jumps(i_jump) > i_from & jumps(i_jump) <= i_to then
-        continue;
-    end
-
-    //Found the best fitting line over the window. Build the system: Ax=0. Matrix A = a_ij
-    a_00 = sum( points_w(1,:).^2 );
-    a_01 = sum( points_w(1,:).*points_w(2,:) );
-    a_02 = sum( points_w(1,:) );
-    a_10 = a_01;
-    a_11 = sum( points_w(2,:).^2 );
-    a_12 = sum( points_w(2,:) );
-    a_20 = a_02;
-    a_21 = a_12;
-    a_22 = Nw;
-    A1 = [a_00 a_01 a_02; a_10 a_11 a_12; a_20 a_21 a_22; 0 0 1];
-    
-    a_00 = sum( points_w(1,:).^2 );
-    a_01 = sum( points_w(1,:).*points_w(2,:) );
-    a_02 = sum( points_w(1,:) );
-    a_10 = a_01;
-    a_11 = sum( points_w(2,:).^2 );
-    a_12 = sum( points_w(2,:) );
-    A = [a_00 a_01 a_02; a_10 a_11 a_12; 0 0 1];
-    
-    //solve
-    line1 = pinv(A1)*[zeros(3,1);1];
-    line = inv(A)*[0; 0; 1];
-    //disp(line1-line);
-
-    //compute error
-    err = 0;
-    for j=1:Nw
-        err = err + abs(line'*[points_w(:,j);1])/sqrt(line(1)^2+line(2)^2);
-    end
-    err = err/Nw;
-    //disp("error: "); disp(err);
-    
-    //if error below stdev, add line to result set
-    if err < K*r_stdev then
-        result_lines = [result_lines [line;points_w(:,1);points_w(:,$)]];
-        line_indexes = [line_indexes [i_from; i_to]]; //ray index where the segment ends
-    end    
-end
-
-//line concatenation
-j = 1;
-while (j < size(result_lines,2))
-    // beams between last of current line and first of next line
-    if (line_indexes(1,j+1)-line_indexes(2,j)) > max_beam_dist then
-        j=j+1;
-    else    
-        //compute angle diff between consecutive segments
-        cos_theta = result_lines(1:2,j)'*result_lines(1:2,j+1) / ( norm(result_lines(1:2,j)) * norm(result_lines(1:2,j+1)) );
-        theta = abs(acos(cos_theta));
-        
-        //if angle diff lower than threshold, concatenate
-        if theta < theta_max then
-            
-            //set the new window
-            i_from = line_indexes(1,j);
-            i_to = line_indexes(2,j+1);
-            points_w = points(:,i_from:i_to);
-        
-            //Found the best fitting line over the window. Build the system: Ax=0. Matrix A = a_ij           
-            a_00 = sum( points_w(1,:).^2 );
-            a_01 = sum( points_w(1,:).*points_w(2,:) );
-            a_02 = sum( points_w(1,:) );
-            a_10 = a_01;
-            a_11 = sum( points_w(2,:).^2 );
-            a_12 = sum( points_w(2,:) );
-            A = [a_00 a_01 a_02; a_10 a_11 a_12; 0 0 1];
-            
-            //solve
-            line = inv(A)*[0; 0; 1];
-        
-            //compute error
-            err = 0;
-            for k=1:Nw
-                err = err + abs(line'*[points_w(:,k);1])/sqrt(line(1)^2+line(2)^2);
-            end
-            err = err/Nw;
-            
-            //if error below stdev, change line to concatenation and erase the next line
-            if err < K*r_stdev then
-                result_lines(:,j) = [line;points_w(:,1);points_w(:,$)];
-                line_indexes(:,j) = [i_from; i_to];
-                result_lines = [result_lines(:,1:j) result_lines(:,j+2:$)];
-                line_indexes = [line_indexes(:,1:j) line_indexes(:,j+2:$)];
-                if (i_to-i_from)>max_points_line then
-                    j=j+1;
-                end
-            else
-                j=j+1;
-            end    
-        else
-            j=j+1;
-        end
-    end
-end
-
-//corner detection
-for i = 1:(size(result_lines,2)-1)
-    for j = i+1:size(result_lines,2)
-        // beams between last of current line and first of next line
-        if (line_indexes(1,j)-line_indexes(2,i)) > max_beam_dist then
-            break;
-        end
-        //compute angle diff between consecutive segments
-        cos_theta = result_lines(1:2,i)'*result_lines(1:2,j) / ( norm(result_lines(1:2,i)) * norm(result_lines(1:2,j)) );
-        theta = abs(acos(cos_theta));
-        
-        //if angle diff greater than threshold && indexes are less than Nw, we decide corner
-        if theta > theta_th then
-            //Corner found! Compute "sub-pixel" corner location as the intersection of two lines
-            corner = cross(result_lines(1:3,i),result_lines(1:3,j));
-            corner = corner./corner(3);//norlamlize homogeneous point
-            // Check if the corner is close to both lines ends
-            if (norm(corner(1:2)-points(:,line_indexes(2,i))) < max_dist & norm(corner(1:2)-points(:,line_indexes(1,j))) < max_dist) then
-                corners = [corners corner];
-            end
-            //display
-            //disp("theta: "); disp(theta);
-            //disp("index:" ); disp(line_indexes(i)-Nw+1);//line_indexes(i) indicates the end point of the segment
-        end
-    end
-end
-
-// corner detection from jumps
-for i=1:size(jumps,2)
-    if ranges(jumps(i)) < ranges(jumps(i)-1) then
-        corners_jumps = [corners_jumps points(:,jumps(i))];
-    else
-        corners_jumps = [corners_jumps points(:,jumps(i)-1)];
-    end
-end
-    
-//Set figure
-fig1 = figure(0);
-fig1.background = 8;
-
-//plot points
-plot(points(1,:),points(2,:),"g.");
-
-//axis settings
-ah = gca();
-ah.isoview = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.grid = [0.1,0.1,0.1];
-ah.grid_position = "background";
-
-
-//plot lines
-for i=1:size(result_lines,2)
-    m = -result_lines(1,i)/result_lines(2,i);
-    xc = -result_lines(3,i)/result_lines(2,i);
-    point1 = [result_lines(4,i) m*result_lines(4,i)+xc];
-    point2 = [result_lines(6,i) m*result_lines(6,i)+xc];
-    xpoly([point1(1) point2(1)],[point1(2) point2(2)]);
-end
-
-//plot corners
-plot(corners(1,:),corners(2,:),"ro");
-plot(corners_jumps(1,:),corners_jumps(2,:),"bo");
-
-disp(corners_jumps');
-disp(corners');
-
-//plot jumps
-//for i=1:size(jumps,2)
-//    plot(ranges(jumps(i))*cos(aperture/2 - azimuth_step*jumps(i)), ranges(jumps(i))*sin(aperture/2 - azimuth_step*jumps(i)),"bx");
-//end
-
-

scilab/scan.txt

-2.78886,2.78289,2.7832,2.78367,2.7843,2.78508,2.78603,2.78713,2.78839,2.78981,2.79139,2.78669,2.78855,2.79057,2.79274,2.79507,2.79098,2.79359,2.79635,2.79927,2.79566,2.79886,2.80221,2.79895,2.80258,2.80638,2.80345,2.80753,2.81176,2.80917,2.81368,2.81132,2.81611,2.81396,2.81903,2.73664,2.72743,2.71782,2.70883,2.69945,2.69067,2.6815,2.67294,2.66398,2.65562,2.64736,2.6387,2.63064,2.62218,2.61431,2.60605,2.59837,2.59078,2.5828,2.57539,2.56759,2.56036,2.55321,2.54568,2.53871,2.53182,2.52455,2.51782,2.51118,2.50415,2.49767,2.49127,2.48495,2.47824,2.47207,2.46597,2.4595,2.45355,2.44768,2.44188,2.4357,2.43005,2.42446,2.41894,2.4135,2.40768,2.40236,2.39712,2.39195,2.3864,2.38135,2.37637,2.37146,2.36661,2.36182,2.35666,2.352,2.3474,2.34286,2.34186,2.36289,2.41051,2.43311,2.45628,2.48003,2.50439,2.52937,2.555,2.61224,2.63993,2.66837,2.69757,2.72758,2.75841,2.79011,2.82269,2.85621,2.8907,2.96659,3.0042,3.04295,3.08289,3.12406,3.16652,3.21033,3.22693,3.23444,3.24207,3.24982,3.2577,3.26571,3.2855,3.29383,3.30229,3.31088,3.3196,3.32846,3.33745,3.34658,3.35584,3.36524,3.37478,3.38445,3.39427,3.40423,3.41434,3.42459,3.43499,3.44553,3.45623,3.46707,3.47807,3.48923,3.50053,3.512,3.52362,3.53541,3.54736,3.55947,3.57175,3.5842,3.59681,3.6096,3.62256,3.63569,3.64901,3.6625,3.67617,3.69003,3.70407,3.7183,3.73272,3.74733,3.76214,3.7931,3.80843,3.82397,3.83972,3.85567,3.87183,3.88822,3.90481,3.92163,3.93867,3.95593,3.97343,3.97347,3.99127,4.00931,4.02758,4.0461,4.06486,4.08387,4.10314,4.12266,4.14244,4.16248,4.20237,4.22313,4.24418,4.26551,4.28712,4.30903,4.33122,4.35372,4.37652,4.39963,4.42304,4.44678,4.47084,4.49523,4.51994,4.545,4.57041,4.59615,4.62225,4.64871,4.67554,4.70274,4.73032,4.75828,4.78663,4.81538,4.84452,4.8741,4.90409,4.9345,4.96534,4.99662,5.02836,5.06053,5.09317,5.12628,5.15987,5.19395,5.22853,5.2636,5.2992,5.33532,5.37197,5.44106,5.47923,5.51796,5.55729,5.59721,5.63773,5.67888,5.72066,5.76307,5.80615,5.8499,5.89432,5.93945,6.02374,6.07085,6.11872,6.16734,6.21676,6.26698,6.318,6.36987,6.42258,6.47618,6.5758,6.6319,6.68894,6.74694,6.80593,6.86593,6.92698,7.04022,7.10425,7.1694,7.23572,7.30322,7.37192,7.49928,7.57149,7.64505,7.58372,7.51951,7.45681,7.32129,7.32938,7.34276,7.35632,7.36877,7.38272,7.39687,7.41124,7.4258,7.43923,7.4542,7.46937,7.48477,7.49904,7.51485,7.53087,7.54579,7.56225,7.57894,7.59587,7.61164,7.62902,8.37389,8.39194,8.41173,8.43177,8.45059,8.47118,8.49202,8.51164,8.53305,8.55319,8.57516,8.59739,8.61839,8.64122,8.6628,8.6862,8.70837,8.73237,8.7567,8.77979,8.80472,8.82843,8.85402,8.87835,8.9046,8.9296,8.9565,8.98218,9.0082,9.03616,9.06287,9.09152,9.11895,9.14834,9.17652,9.20503,9.23557,9.26487,9.29454,9.32626,9.35674,9.38762,9.42055,9.45226,9.4844,9.51695,9.55162,9.58503,9.61893,9.65324,4.38929,4.38536,4.36058,4.3365,4.3131,4.29036,4.26827,4.24682,4.22598,4.20576,4.18612,4.1944,4.17582,4.15708,4.13859,4.12032,4.10229,4.08449,4.06691,4.04955,4.03241,4.01549,3.99916,3.98265,3.96634,3.95024,3.93434,3.91901,3.90349,3.88817,3.87304,3.85845,3.84368,3.82909,3.81504,3.80081,3.78674,3.7732,3.75948,3.74591,3.73287,3.71963,3.7069,3.69398,3.68156,3.66894,3.65647,3.6445,3.63233,3.62065,3.60876,3.59736,3.58576,3.58265,3.61553,3.62696,3.63867,3.67347,3.68596,3.72229,3.7356,3.77355,3.78772,3.80219,3.84244,3.85785,3.89993,3.9163,3.93303,3.97774,3.99551,4.01367,4.06121,4.0805,4.10019,4.15081,4.17174,4.19309,13.8062,13.7714,13.7384,13.7075,13.8936,13.9735,14.0549,14.1382,14.3407,15.8017,15.9009,16.002,16.1054,16.3519,16.462,16.5744,16.6893,16.9594,17.0819,17.2072,17.3352,17.4661,17.6,8.14878,8.1334,8.11823,8.10324,8.08848,8.07391,8.0588,8.04465,8.03069,8.01693,8.00338,7.99,7.97684,7.96312,7.95032,7.93773,7.92533,7.91309,7.90106,7.88922,7.87755,7.86606,7.85476,7.84289,7.83195,7.82116,7.81058,7.80017,7.78993,7.77984,7.76995,7.76021,7.75066,7.74128,7.73204,7.76034,7.99805,8.11853,8.24311,8.37202,12.3718,12.3587,12.346,12.3336,12.3213,12.3094,12.2976,12.2862,12.275,12.264,12.2534,12.2429,12.2327,12.2228,12.213,12.2036,12.1944,12.1854,12.1766,12.3577,12.667,16.7608,16.7501,16.7398,16.7297,16.7201,16.7106,16.7015,16.6929,16.6844,16.9488,20.069,20.0619,20.0552,20.0489,20.043,20.0374,20.0323,20.0275,20.0231,20.0191,20.0155,20.0122,20.0094,20.0069,20.0048,20.0031,20.0018,20.0008,20.0002,20.0001,20.0002,20.0008,20.0018,20.0031,20.0048,20.0069,20.0094,20.0122,20.0155,20.0191,20.0231,20.0275,20.0323,20.0374,20.043,20.0489,20.0552,20.0619,20.069,20.0764,20.0843,20.0926,20.1012,20.1102,20.1196,20.1294,20.1397,20.1502,20.1612,20.1726,20.1844,20.1966,20.2092,20.2222,20.2356,20.2494,20.2636,20.2782,20.2932,20.3086,20.3244,20.3407,20.3573,20.3744,20.3919,20.4098,20.4281,20.4469,20.466,20.4856,20.5057,20.5261,20.547,20.5684,20.5901,20.6123,20.635,20.6581,20.6816,20.7056,20.73,20.7549,20.7802,20.806,20.8323,20.859,20.8862,20.9139,20.942,20.9706,20.9997,21.0293,21.0594,21.0899,21.1209,21.1525,21.1845,21.217,21.2501,21.2836,21.3177,21.3522,21.3873,21.423,21.4591,21.4958,21.533,21.5707,21.6091,21.6479,21.6873,21.7273,21.7678,21.8089,21.8505,21.8928,21.9356,21.979,22.023,22.0676,22.1128,22.1586,22.2051,22.2521,22.2998,22.3481,22.397,22.4466,22.4968,22.5477,22.5992,22.6515,22.7043,22.7579,22.8122,22.8671,22.9228,22.9792,23.0363,23.0941,23.1526,23.2119,23.2719,23.3327,23.3943,23.4566,23.5197,23.5836,23.6483,23.7138,23.7802,23.8473,23.9153,23.9842,24.0539,24.1244,24.1959,24.2682,24.3414,24.4156,24.4906,24.5666,24.6435,24.7214,24.8003,24.8801,24.9609,25.0428,25.1256,25.2095,25.2944,25.3804,25.4675,25.5556,25.6449,25.7353,25.8268,25.9194,26.0132,26.1082,26.2044,26.3018,26.4004,26.5003,26.6015,26.7039,26.8077,26.9127,27.0191,27.1269,27.2361,27.3466,27.4586,27.572,27.6869,27.8033,27.9213,28.0407,28.1617
\ No newline at end of file

scilab/test_ceres_odom_plot.sce

-//plot log data from ceres test
-
-// clear all 
-xdel(winsid());
-clear;
-
-// CERES ODOM BATCH
-//load log file
-data = read('~/Desktop/log_file_2.txt',-1,14);
-
-//plot
-fig1 = figure(0);
-fig1.background = 8;
-plot(data(2:$,10),data(2:$,11),"g.");
-plot(data(2:$,1),data(2:$,2),"b-");
-plot(data(2:$,4),data(2:$,5),"r-");
-plot(data(2:$,13),data(2:$,14),"c--");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$GPS$";"$Optimization$";"$Ground\ Truth$";"$ODOM$"],4);
-lh.font_size = 3;
-title(strcat(["CERES_ODOM_BATCH - Time: ",string(data(1,1))," s"]));
-ah.title.font_size = 4;
-
-// MANAGER - THETA
-//load log file
-data2 = read('~/Desktop/log_file_2.txt',-1,15);
-data2L = read('~/Desktop/landmarks_file_2.txt',-1,2);
-
-disp(data2L);
-
-//plot
-fig2 = figure(1);
-fig2.background = 8;
-//plot(data2(2:$,13),data2(2:$,14),"g.");
-plot(data2(2:$,1),data2(2:$,2),"b-");
-plot(data2(2:$,4),data2(2:$,5),"r-");
-plot(data2(2:$,10),data2(2:$,11),"c--");
-
-plot(data2L(1:$,1),data2L(1:$,2),"k.");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$Optimization$";"$Ground\ Truth$";"$ODOM$";"$Landmarks$"],4);
-lh.font_size = 3;                             
-title(strcat(["CERES_MANAGER: Theta - Time: ",string(data2(1,1))," s"]));
-ah.title.font_size = 4;
-
-// MANAGER - COMPLEX ANGLE
-//load log file
-data3 = read('~/Desktop/log_file_3.txt',-1,15);
-
-//plot
-fig3 = figure(2);
-fig3.background = 8;
-plot(data3(2:$,13),data3(2:$,14),"g.");
-plot(data3(2:$,1),data3(2:$,2),"b-");
-plot(data3(2:$,4),data3(2:$,5),"r-");
-plot(data3(2:$,10),data3(2:$,11),"c--");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$GPS$";"$Optimization$";"$Ground\ Truth$";"$ODOM$"],4);
-lh.font_size = 3;                             
-title(strcat(["CERES_MANAGER: Complex Angle - Time: ",string(data3(1,1))," s"]));
-ah.title.font_size = 4;
-

wolf_scripts/license_manager.sh → scripts/license_manager.sh

@@ -6,16 +6,16 @@
 # This file is automatically called by the CI in gitlab.
 echo "==== WOLF license_manager script ===="
 
-line_start_mark="//--------LICENSE_START--------"
-line_end_mark="//--------LICENSE_END--------"
-
 #options
 tmp=false
 mode="none"
 path=""
 exclude_folder=""
 #recursive=true
-license=""
+#license=""
+config_folder=""
+license_exclude=""
+exclude_mark="// This is not part of WOLF"
 
 for i in "$@"; do
   case $i in
@@ -27,22 +27,22 @@ for i in "$@"; do
       path="${i#*=}"
       shift # past argument=value
       ;;
-    --license-header=*)
-      license="${i#*=}"
+    --config-path=*)
+      config_folder="${i#*=}"
       shift # past argument=value
       ;;
     --add)
       mode="add"
-      if [ $mode == "update" ]; then
-        echo "Error: Script cannot be called with both options: --update or --add"
+      if [ $mode == "remove" ]; then
+        echo "Script cannot be called with both options: --remove or --add">&2
         exit 1
       fi
       shift # past argument=value
       ;;
-    --update)
-      mode="update"
+    --remove)
+      mode="remove"
       if [ $mode == "add" ]; then
-        echo "Error: Script cannot be called with both options: --update or --add"
+        echo "Script cannot be called with both options: --remove or --add">&2
         exit 1
       fi
       shift # past argument=value
@@ -57,29 +57,45 @@ for i in "$@"; do
   esac
 done
 
-# check options
+# CHECKS VALID OPTIONS =================================
 if [ "$path" == "" ]; then
-  echo 'Please, provide the path to the folder containing the code with --path=/my/folder/code'
+  echo 'Please, provide the path to the folder containing the code with --path=/my/folder/code'>&2
   exit 1
 else
   if [ -d "$path" ]; then
     echo "Valid path: ${path}"
   else
-    echo "Error: ${path} not found. Can not continue."
-  exit 1
+    echo "${path} not found. Can not continue.">&2
+    exit 1
   fi
 fi
 
-if [ "$license" == "" ]; then
-  echo 'Error: Please, provide the path to the folder containing the code with --license-header=/my/license/header/file.txt'
+if [ "$config_folder" == "" ]; then
+  echo 'Please, provide the path to the folder containing the config files (license file "license_header*.txt" and the optional "license_exclude.txt") with --config-path=/my/license/and/exclude/files/folder'>&2
   exit 1
 else
-  if [ -f "$license" ]; then
-    echo "Valid license header file: ${license} containing:"
-    cat ${license}
+  if [ -d "$config_folder" ]; then
+    echo "Valid config folder: ${config_folder}"
+
+    license=$(find $config_folder -maxdepth 1 -name 'license_header*.txt')
+    if (( ${#license[@]} != 1 )); then
+      echo "No license header file found" >&2
+    fi
+    if [ -f "$license" ]; then
+      echo "Valid license header file: ${license} containing:"
+      cat ${license}
+    else
+      echo "License header file ${license} not found.">&2
+      exit 1
+    fi
+    if [ -f "${config_folder}/license_exclude.txt" ]; then
+      echo "License_exclude file containing:"
+      cat ${config_folder}/license_exclude.txt
+      echo ""
+    fi
   else
-    echo "Error: License header file ${license} not found. Can not continue."
-  exit 1
+    echo "Config path ${config_folder} not found.">&2
+    exit 1
   fi
 fi
 
@@ -87,17 +103,30 @@ if [ "$exclude_folder" == "" ]; then
   echo "No folders will be excluded"
 else
   if [ -d "${path}/${exclude_folder}" ]; then
-    echo "Valid remove folder path: ${path}/${exclude_folder}"
+    echo "Valid exclude folder path: ${path}/${exclude_folder}"
+  else
+    echo "exclude folder ${path}/${exclude_folder} not found. Remember that it should be relative to --path.">&2
+    exit 1
   fi
 fi
 
 if [ $mode == "none" ]; then
-  echo "Error: Script should be called with one of the following options: --update or --add"
+  echo "Script should be called with one of the following options: --remove or --add">&2
   exit 1
 else
   echo "mode: ${mode}"
 fi
 
+# START SCRIPT ==========================================
+
+# DETECT FIRST AND LAST LICENSE LINES
+sed -i -e '/./,$!d' -e :a -e '/^\n*$/{$d;N;ba' -e '}' $license #remove empty lines at the beginning and at the end
+line_start_mark=$(head -n 1 $license)
+line_end_mark=$(tail -n 1 $license)
+echo "line_start: $line_start_mark"
+echo "line_end: $line_end_mark"
+echo "excluding files starting with: $exclude_mark"
+
 # PATH (AND tmp FOLDER)
 folder=$path
 if [ $tmp == true ]; then
@@ -111,36 +140,46 @@ fi
 if [ "$exclude_folder" == "" ]; then
   file_list=$(find $folder -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
 else
-  file_list=$(find $folder -path ${folder}/${exclude_folder} -prune -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
+  file_list=$(find $folder -path ${path}/${exclude_folder} -prune -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
+fi
+
+# FILTER FILES IF LICENSE EXCLUDE
+if [ -f "${config_folder}/license_exclude.txt" ]; then
+  file_list=$(grep -Ev -f ${config_folder}/license_exclude.txt <(printf "%s\n" "${file_list[@]}"))
 fi
+# printf "%s\n" "${file_list[@]}"
 
 # DETECT AND REMOVE EXISTING LICENSE
-if [ "$mode" == "update" ]; then
+if [ "$mode" == "remove" ]; then
   echo "Recursely removing license header from all files (.c, .cpp, .h, .hpp):"
   for i in $file_list
   do
-    if grep -Fxq ${line_start_mark} $i 
-    then
-      echo " - ${i}"
-      line_start="$(grep -wn $line_start_mark ${i} | head -n 1 | cut -d: -f1)"
-      line_end="$(grep -wn $line_end_mark ${i} | head -n 1 | cut -d: -f1)"
-      #echo ${line_start}
-      #echo ${line_end}
-      awk -v m=$line_start -v n=$line_end 'm <= NR && NR <= n {next} {print}' $i > tmpfile && mv tmpfile $i 
-      #cat $i
+    if grep -m1 -Fxq "${line_start_mark}" $i; then
+      line_start="$(grep -Fxn "$line_start_mark" ${i} | head -n 1 | cut -d: -f1)"
+      line_end="$(grep -Fxn "$line_end_mark" ${i} | head -n 1 | cut -d: -f1)"
+      if [ -n "$line_start" ] && [ -n "$line_end" ]; then
+        echo "${i} contains license header: L${line_start} - L${line_end}"
+        awk -v m=$line_start -v n=$line_end 'm <= NR && NR <= n {next} {print}' $i > tmpfile && mv tmpfile $i 
+      fi
     fi
   done
 fi
 
 # ADD CONTENT OF license-file AT THE BEGINNING OF CODE FILES
-echo "Recursively adding license header to all files (.c, .cpp, .h, .hpp):"
-for i in $file_list
-do
-  if grep -Fxq ${line_start_mark} $i; then
-    :
-  else
-    echo " - ${i}"
-    ( echo ${line_start_mark}$'\n//'; cat ${license}; echo $'//\n'${line_end_mark}; cat $i ) > temp_file
-    mv temp_file $i
-  fi
-done
+if [ "$mode" == "add" ]; then
+  echo "Recursively adding license header to all files (.c, .cpp, .h, .hpp):"
+  for i in $file_list
+  do
+    if grep -m1 -Fxq "${exclude_mark}" $i; then
+      echo "excluding ${i} since it contains exclude mark"
+    else
+      if grep -m1 -Fxq "${line_start_mark}" $i; then
+        :
+      else
+        echo " - ${i}"
+        ( cat ${license}; echo ""; cat $i ) > temp_file
+        mv temp_file $i
+      fi
+    fi
+  done
+fi

src/capture/capture_base.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "core/capture/capture_base.h"
 #include "core/sensor/sensor_base.h"
+#include "core/factor/factor_block_difference.h"
 
-namespace wolf{
-
+namespace wolf
+{
 using namespace Eigen;
 
 unsigned int CaptureBase::capture_id_count_ = 0;
 
-CaptureBase::CaptureBase(const std::string& _type,
-                         const TimeStamp& _ts,
-                         SensorBasePtr _sensor_ptr,
-                         StateBlockPtr _p_ptr,
-                         StateBlockPtr _o_ptr,
-                         StateBlockPtr _intr_ptr) :
-    NodeBase("CAPTURE", _type),
-    HasStateBlocks(""),
-    frame_ptr_(), // nullptr
-    sensor_ptr_(_sensor_ptr),
-    capture_id_(++capture_id_count_),
-    time_stamp_(_ts)
+CaptureBase::CaptureBase(const std::string&     _type,
+                         const TimeStamp&       _ts,
+                         SensorBasePtr          _sensor_ptr,
+                         const TypeComposite&   _state_types,
+                         const VectorComposite& _state_vectors,
+                         const PriorComposite&  _state_priors)
+    : NodeStateBlocks("CAPTURE", _type, _state_types, _state_vectors, _state_priors),
+      frame_ptr_(),  // nullptr
+      sensor_ptr_(_sensor_ptr),
+      capture_id_(++capture_id_count_),
+      time_stamp_(_ts)
 {
     assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!");
-    if (_sensor_ptr)
-    {
-        if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
-        {
-            WOLF_ERROR_COND(not _p_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state P dynamic but provided _p_ptr is nullptr")
-            assert(_p_ptr && "Pointer to dynamic position params is null!");
-            addStateBlock('P', _p_ptr, nullptr);
-        }
-        else
-            assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
 
-        if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
-        {
-            WOLF_ERROR_COND(not _o_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state O dynamic but provided _o_ptr is nullptr")
-            assert(_o_ptr && "Pointer to dynamic orientation params is null!");
-            addStateBlock('O', _o_ptr, nullptr);
-        }
-        else
-            assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
+    // states provided but no sensor
+    WOLF_ERROR_COND(not _sensor_ptr and not _state_vectors.empty(),
+                    "Type " + _type + ": sensor not provided (nullptr) but states provided");
 
-        if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
+    // Check that provided states are dynamic
+    if (_sensor_ptr)
+    {
+        for (auto pair_vector : _state_vectors)
         {
-            WOLF_ERROR_COND(not _intr_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state I dynamic but provided _i_ptr is nullptr")
-            assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
-            addStateBlock('I', _intr_ptr, nullptr);
+            char key = pair_vector.first;
+            // missing state
+            if (not _sensor_ptr->getStateBlock(key))
+                throw std::runtime_error(std::string("In CaptureBase constructor of type ") + _type + " the sensor " +
+                                         _sensor_ptr->getType() + " " + _sensor_ptr->getName() +
+                                         " do not have state " + std::string(1, key));
+            // static state
+            if (not _sensor_ptr->isStateBlockDynamic(key))
+                throw std::runtime_error(std::string("In CaptureBase constructor of type ") + _type + " the sensor " +
+                                         _sensor_ptr->getType() + " " + _sensor_ptr->getName() + " state " +
+                                         std::string(1, key) + " is static");
+            // wrong type
+            if (_state_types.at(key) != _sensor_ptr->getStateTypes().at(key))
+                throw std::runtime_error(std::string("In CaptureBase constructor of type ") + _type + " the sensor " +
+                                         _sensor_ptr->getType() + " " + _sensor_ptr->getName() + " state " +
+                                         std::string(1, key) + " has type " + _sensor_ptr->getStateTypes().at(key) +
+                                         " (provided " + _state_types.at(key) + ")");
         }
-        else
-            assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
-
-    }
-    else if (_p_ptr || _o_ptr || _intr_ptr)
-    {
-        WOLF_ERROR("Provided sensor parameters but no sensor pointer");
     }
 }
 
-CaptureBase::~CaptureBase()
-{
-}
-
-void CaptureBase::remove(bool viral_remove_empty_parent)
+void CaptureBase::remove(bool viral_remove_parent_without_children)
 {
     /* Order of removing:
      * Factors are removed first, and afterwards the rest of nodes containing state blocks.
      * In case multi-threading, solver can be called while removing.
      * This order avoids SolverManager to ignore notifications (efficiency)
      */
-    if (!is_removing_)
-    {
-        is_removing_ = true;
-        CaptureBasePtr this_C = shared_from_this();  // keep this alive while removing it
+    if (is_removing_) return;
 
-        // SensorBase::last_capture_
-        if (getSensor() and getSensor()->getLastCapture() == this_C)
-            getSensor()->updateLastCapture();
+    is_removing_          = true;
+    CaptureBasePtr this_C = shared_from_this_capture();  // keep this alive while removing it
 
-        // remove downstream
-        while (!constrained_by_list_.empty())
-        {
-            constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
-        }
-        while (!feature_list_.empty())
-        {
-            feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
-        }
+    // SensorBase::last_capture_
+    if (getSensor() and getSensor()->getLastCapture() == this_C) getSensor()->updateLastCapture();
 
-        // Remove State Blocks
-        removeStateBlocks(getProblem());
+    // remove downstream
+    while (not feature_list_.empty())
+    {
+        feature_list_.front()->remove(viral_remove_parent_without_children);  // remove downstream
+    }
 
-        // remove from upstream
-        FrameBasePtr F = frame_ptr_.lock();
-        if (F)
+    // remove from upstream
+    FrameBasePtr F = frame_ptr_.lock();
+    if (F)
+    {
+        F->removeCapture(this_C);
+        if (viral_remove_parent_without_children and not F->hasChildren())
         {
-            F->removeCapture(this_C);
-            if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
-                F->remove(viral_remove_empty_parent); // remove upstream
+            F->remove(viral_remove_parent_without_children);  // remove upstream
         }
     }
+
+    // Remove State Blocks
+    NodeStateBlocks::remove(viral_remove_parent_without_children);
+}
+
+bool CaptureBase::hasChildren() const
+{
+    return NodeStateBlocks::hasChildren() or not feature_list_.empty();
+}
+
+FactorBasePtr CaptureBase::emplaceDriftFactor(CaptureBasePtr _capture_origin, char _key, bool _apply_loss_function)
+{
+    // Checks
+    if (not getSensor())  // sensor exists
+        throw std::runtime_error("Attempting to call emplaceDriftFactor to a capture with no associated sensor.");
+    if (not _capture_origin)  //_capture_origin is null
+        throw std::runtime_error("Attempting to call emplaceDriftFactor to a null capture origin.");
+    if (_capture_origin == shared_from_this_capture())  //_capture_origin is different than this
+        throw std::runtime_error("Attempting to call emplaceDriftFactor to the same capture.");
+    if (not getStateBlock(_key) or not _capture_origin->getStateBlock(_key))  // states exist
+        throw std::runtime_error("Attempting to call emplaceDriftFactor state does not exist in any of the captures.");
+    if (not getSensor()->isStateBlockDynamic(_key))  // state is dynamic
+        throw std::runtime_error("Attempting to call emplaceDriftFactor to a non-dynamic state.");
+    if (not getSensor()->hasDrift(_key))  // drift specified
+        throw std::runtime_error("Attempting to call emplaceDriftFactor to a state without drift specified.");
+
+    // Feature drift
+    auto dt       = getTimeStamp() - _capture_origin->getTimeStamp();
+    auto ftr_bias = FeatureBase::emplace<FeatureBase>(
+        shared_from_this_capture(),
+        "FeatureBase",
+        VectorXd::Zero(getStateBlock(_key)->getSize()),                                 // mean drift is zero
+        MatrixXd((getSensor()->getDriftStdRate(_key) * dt).cwiseAbs2().asDiagonal()));  // drift cov specified in
+                                                                                        // continuous time
+
+    // FactorBlockDifference (whole state)
+    return FactorBase::emplace<FactorBlockDifference>(ftr_bias,
+                                                      ftr_bias->getMeasurement(),
+                                                      ftr_bias->getMeasurementSquareRootInformationUpper(),
+                                                      _capture_origin,
+                                                      shared_from_this_capture(),
+                                                      _key,                   // key of the 1st state block
+                                                      _key,                   // key of the 2nd state block
+                                                      nullptr,                // processor
+                                                      _apply_loss_function);  // loss function
 }
 
 bool CaptureBase::process()
 {
-    assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
+    assert(getSensor() != nullptr &&
+           "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call "
+           "sensor->process(capture) instead.");
 
-    return getSensor()->process(shared_from_this());
+    return getSensor()->process(shared_from_this_capture());
 }
 
 FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
 {
-    //std::cout << "Adding feature" << std::endl;
     feature_list_.push_back(_ft_ptr);
     return _ft_ptr;
 }
@@ -140,7 +167,6 @@ void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
 {
     feature_list_.remove(_ft_ptr);
 }
-
 FactorBaseConstPtrList CaptureBase::getFactorList() const
 {
     FactorBaseConstPtrList fac_list;
@@ -158,83 +184,67 @@ FactorBasePtrList CaptureBase::getFactorList()
 void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
 {
     for (auto f_ptr : getFeatureList())
-        if (not f_ptr->isRemoving())
-            f_ptr->getFactorList(_fac_list);
+        if (not f_ptr->isRemoving()) f_ptr->getFactorList(_fac_list);
 }
 
 void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
 {
     for (auto f_ptr : getFeatureList())
-        if (not f_ptr->isRemoving())
-            f_ptr->getFactorList(_fac_list);
-}
-
-FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
-    constrained_by_list_.push_back(_fac_ptr);
-    return _fac_ptr;
-}
-
-void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
-{
-    constrained_by_list_.remove(_fac_ptr);
+        if (not f_ptr->isRemoving()) f_ptr->getFactorList(_fac_list);
 }
 
-bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const
+bool CaptureBase::hasFeature(const FeatureBaseConstPtr& _feature) const
 {
-    return std::find(feature_list_.begin(),
-                     feature_list_.end(),
-                     _feature) != feature_list_.end();
-}
-
-bool CaptureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
-{
-    return std::find(constrained_by_list_.begin(),
-                     constrained_by_list_.end(),
-                     _factor) != constrained_by_list_.end();
+    return std::find(feature_list_.begin(), feature_list_.end(), _feature) != feature_list_.end();
 }
 
 StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
 {
-    if (getSensor() and getSensor()->hasStateBlock(_key))
+    if (getSensor() and getSensor()->has(_key))
     {
         if (getSensor()->isStateBlockDynamic(_key))
-            return HasStateBlocks::getStateBlock(_key);
+            return NodeStateBlocks::getStateBlock(_key);
         else
             return getSensor()->getStateBlock(_key);
     }
-    else // No sensor associated, or sensor without this state block: assume sensor params are here
-        return HasStateBlocks::getStateBlock(_key);
+    else  // No sensor associated, or sensor without this state block: assume sensor params are here
+        return NodeStateBlocks::getStateBlock(_key);
 }
 
 StateBlockPtr CaptureBase::getStateBlock(const char& _key)
 {
-    if (getSensor() and getSensor()->hasStateBlock(_key))
+    if (getSensor() and getSensor()->has(_key))
     {
         if (getSensor()->isStateBlockDynamic(_key))
-            return HasStateBlocks::getStateBlock(_key);
+            return NodeStateBlocks::getStateBlock(_key);
         else
             return getSensor()->getStateBlock(_key);
     }
-    else // No sensor associated, or sensor without this state block: assume sensor params are here
-        return HasStateBlocks::getStateBlock(_key);
+    else  // No sensor associated, or sensor without this state block: assume sensor params are here
+        return NodeStateBlocks::getStateBlock(_key);
 }
 
 void CaptureBase::fix()
 {
-    HasStateBlocks::fix();
+    NodeStateBlocks::fix();
 }
 
 void CaptureBase::unfix()
 {
-    HasStateBlocks::unfix();
+    NodeStateBlocks::unfix();
 }
 
 void CaptureBase::move(FrameBasePtr _frm_ptr)
 {
-    WOLF_WARN_COND(this->getFrame() == nullptr, "Moving Capture ", id(), " at ts=", getTimeStamp(), " not linked to any frame. Consider just linking it with link() instead of move()!");
+    WOLF_WARN_COND(this->getFrame() == nullptr,
+                   "Moving Capture ",
+                   id(),
+                   " at ts=",
+                   getTimeStamp(),
+                   " not linked to any frame. Consider just linking it with link() instead of move()!");
 
-    assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!");
+    assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) &&
+           "Forbidden: trying to move a capture already linked to a KF!");
 
     // Unlink
     unlink();
@@ -250,9 +260,9 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
 
     WOLF_WARN_COND(_frm_ptr == nullptr, "Linking Capture ", id(), " to a nullptr");
 
-    if(_frm_ptr)
+    if (_frm_ptr)
     {
-        _frm_ptr->addCapture(shared_from_this());
+        _frm_ptr->addCapture(shared_from_this_capture());
         this->setFrame(_frm_ptr);
         this->setProblem(_frm_ptr->getProblem());
     }
@@ -260,47 +270,44 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
 
 void CaptureBase::unlink()
 {
-    WOLF_WARN_COND(this->getFrame() == nullptr, "Unlinking a not linked Capture ", id(), ". Nothing to do, skipping...");
+    WOLF_WARN_COND(
+        this->getFrame() == nullptr, "Unlinking a not linked Capture ", id(), ". Nothing to do, skipping...");
 
-    if (not this->getFrame())
-        return;
+    if (not this->getFrame()) return;
 
-    for (auto ftr : getFeatureList())
-        assert(ftr->getFactorList().empty() && " unlinking a capture with factors!");
-    assert(getConstrainedByList().empty() && " unlinking a capture constrained by factors!");
+    assert(getFactorList().empty() && " unlinking a capture constrained by factors!");
 
     // unlink from frame
-    this->getFrame()->removeCapture(shared_from_this());
+    this->getFrame()->removeCapture(shared_from_this_capture());
     this->setFrame(nullptr);
 }
 
 void CaptureBase::setProblem(ProblemPtr _problem)
 {
-    if (_problem == nullptr || _problem == this->getProblem())
-        return;
+    if (_problem == nullptr || _problem == this->getProblem()) return;
 
     assert(getFrame() && "Cannot set problem: Capture has no Frame!");
 
-    NodeBase::setProblem(_problem);
-    registerNewStateBlocks(_problem);
+    NodeStateBlocks::setProblem(_problem);
 
     // update SensorBase::last_capture_
     if (getSensor() and
-        (not getSensor()->getLastCapture() or
-         getSensor()->getLastCapture()->getTimeStamp() < time_stamp_))
-         getSensor()->setLastCapture(shared_from_this());
+        (not getSensor()->getLastCapture() or getSensor()->getLastCapture()->getTimeStamp() < time_stamp_))
+        getSensor()->setLastCapture(shared_from_this_capture());
 
-    for (auto ft : feature_list_)
-        ft->setProblem(_problem);
+    for (auto ft : feature_list_) ft->setProblem(_problem);
 }
 
-void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void CaptureBase::printHeader(int           _depth,
+                              bool          _factored_by,
+                              bool          _metric,
+                              bool          _state_blocks,
+                              std::ostream& _stream,
+                              std::string   _tabs) const
 {
-    _stream << _tabs << "Cap" << id()
-            << " " << getType()
-            << " ts = " << std::setprecision(3) << getTimeStamp();
+    _stream << _tabs << "Cap" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
 
-    if(getSensor() != nullptr)
+    if (getSensor() != nullptr)
     {
         _stream << " -> Sen" << getSensor()->id();
     }
@@ -308,152 +315,105 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
         _stream << " -> Sen-";
 
     _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
-    if (_constr_by)
+    if (_factored_by)
     {
         _stream << "  <-- ";
-        for (auto cby : getConstrainedByList())
-            if (cby)
-                _stream << "Fac" << cby->id() << " \t";
+        for (auto fac : getFactoredBySet())
+            if (fac) _stream << "Fac" << fac->id() << " \t";
     }
     _stream << std::endl;
 
-    if (getStateBlockMap().size() > 0)
-        printState(_metric, _state_blocks, _stream, _tabs);
+    if (getStateBlockMap().size() > 0) printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
 }
 
-void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void CaptureBase::print(int           _depth,
+                        bool          _factored_by,
+                        bool          _metric,
+                        bool          _state_blocks,
+                        std::ostream& _stream,
+                        std::string   _tabs) const
 {
-    printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+    printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 3)
         for (auto f : getFeatureList())
-            if (f)
-                f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (f) f->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
-CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog CaptureBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    CheckLog log;
+    CheckLog          log;
     std::stringstream inconsistency_explanation;
 
     if (_verbose)
     {
-        _stream << _tabs << "Cap" << id() << " @ " << _cap_ptr.get() << " -> Sen";
-        if (getSensor()) _stream << getSensor()->id();
-        else _stream << "-";
+        _stream << _tabs << "Cap" << id() << " @ " << this << " -> Sen";
+        if (getSensor())
+            _stream << getSensor()->id();
+        else
+            _stream << "-";
         _stream << std::endl;
-        _stream << _tabs << "  " << "-> Prb  @ " << getProblem().get() << std::endl;
-        _stream << _tabs << "  " << "-> Frm" << getFrame()->id() << " @ " << getFrame().get() << std::endl;
-    }
-    for (auto pair: getStateBlockMap())
-    {
-        auto sb = pair.second;
-
-        // check for valid state block
-        inconsistency_explanation << "Capture " << id() << " @ " << _cap_ptr.get() << " has State block pointer "
-                                  << sb.get() << " = 0 \n";
-        log.assertTrue((sb.get() != 0), inconsistency_explanation);
-
-        if (_verbose)
-        {
-            _stream <<  _tabs << "  " << pair.first << " sb @ " << sb.get();
-            if (sb) {
-                auto lp = sb->getLocalParametrization();
-                if (lp)
-                    _stream << " (lp @ " << lp.get() << ")";
-            }
-            _stream << std::endl;
-        }
+        _stream << _tabs << "  "
+                << "-> Prb  @ " << getProblem() << std::endl;
+        _stream << _tabs << "  "
+                << "-> Frm" << getFrame()->id() << " @ " << getFrame() << std::endl;
     }
 
-    auto frm_ptr = getFrame();
     // check problem and frame pointers
-    inconsistency_explanation << "Capture problem pointer " << getProblem().get()
-                              << " different from frame problem pointer " << frm_ptr->getProblem().get() << "\n";
+    auto frm_ptr = getFrame();
+    inconsistency_explanation << "Capture problem pointer " << getProblem() << " different from frame problem pointer "
+                              << frm_ptr->getProblem() << "\n";
     log.assertTrue((getProblem() == frm_ptr->getProblem()), inconsistency_explanation);
 
-
-    // check contrained_by
-    for (auto cby : getConstrainedByList())
-    {
-        if (_verbose)
-        {
-            _stream << _tabs << "    " << "<- Fac" << cby->id() << " -> ";
-            for (auto Cow : cby->getCaptureOtherList())
-                _stream << " Cap" << Cow.lock()->id();
-            _stream << std::endl;
-        }
-
-        // check constrained_by pointer to this capture
-        inconsistency_explanation << "constrained by capture " << id() << " @ " << _cap_ptr.get()
-                                  << " not found among constrained-by factors\n";
-        log.assertTrue((cby->hasCaptureOther(_cap_ptr)), inconsistency_explanation);
-
-        for (auto sb : cby->getStateBlockPtrVector())
-        {
-            if (_verbose)
-            {
-                _stream << _tabs << "      " << "sb @ " << sb.get();
-                if (sb)
-                {
-                    auto lp = sb->getLocalParametrization();
-                    if (lp)
-                        _stream <<  " (lp @ " << lp.get() << ")";
-                }
-                _stream << std::endl;
-            }
-        }
-    }
-
     // check frame
-    auto frm_cap = _cap_ptr->getFrame();
-    inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
-                                << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
-                                << " -X-> Frm" << id() << "\n";
-    auto frm_cap_list = frm_cap->getCaptureList();
-    auto frame_has_cap = std::find(frm_cap_list.begin(), frm_cap_list.end(), _cap_ptr);
+    inconsistency_explanation << "Cap" << id() << " @ " << this << " ---> Frm" << frm_ptr->id() << " @ " << frm_ptr
+                              << " -X-> Frm" << id() << "\n";
+    auto frm_cap_list  = frm_ptr->getCaptureList();
+    auto frame_has_cap = std::find(frm_cap_list.begin(), frm_cap_list.end(), shared_from_this_capture());
     log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
 
-
     // check features
-    for(auto f : getFeatureList())
+    for (auto f : getFeatureList())
     {
-        inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
-                                    << " ---> Ftr" << f->id() << " @ " << f
-                                    << " -X-> Cap" << id();
-        log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
+        inconsistency_explanation << "Cap " << id() << " @ " << this << " ---> Ftr" << f->id() << " @ " << f
+                                  << " -X-> Cap" << id();
+        log.assertTrue((f->getCapture() == shared_from_this_capture()), inconsistency_explanation);
     }
 
-    //Check that the capture is within time tolerance of some processor
-    auto frame = getFrame();
+    // Check that the capture is within time tolerance of some processor
+    auto   frame     = getFrame();
     double time_diff = fabs(getTimeStamp() - frame->getTimeStamp());
 
-    //It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
-    //This inicialization is needed because if the list empty the execution does not go into the loop and the
-    //assertion fails
-    bool match_any_prc_timetolerance;
-    if(getSensor() != nullptr )
+    // It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
+    // This initialization is needed because if the list empty the execution does not go into the loop and the
+    // assertion fails
+    bool match_any_prc_timetolerance = false;
+    if (getSensor() != nullptr)
     {
         match_any_prc_timetolerance = getSensor()->getProcessorList().empty();
-        for(auto prc : getSensor()->getProcessorList())
+        for (auto prc : getSensor()->getProcessorList())
         {
             match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
         }
-        inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
-                                    << " ts =" << getTimeStamp() << "Frm" << frame->id()
-                                    << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of"
-                                    << " any processor in sensor " << getSensor()->id() << "\n";
+        inconsistency_explanation << "Cap " << id() << " @ " << this << " ts =" << getTimeStamp() << "Frm"
+                                  << frame->id() << " ts = " << frame->getTimeStamp() << " their time difference ("
+                                  << time_diff << ") does not match any time tolerance of"
+                                  << " any processor in sensor " << getSensor()->id() << "\n";
         log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation);
     }
 
+    // check NodeStateBlocks
+    auto node_log = NodeStateBlocks::localCheck(_verbose, _stream, _tabs);
+    log.compose(node_log);
+
     return log;
 }
-bool CaptureBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+
+bool CaptureBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    auto sen_ptr = std::static_pointer_cast<const CaptureBase>(_node_ptr);
-    auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
+    auto local_log = localCheck(_verbose, _stream, _tabs);
     _log.compose(local_log);
 
-    for(auto f : getFeatureList()) f->check(_log, f, _verbose, _stream, _tabs + "  ");
+    for (auto f : getFeatureList()) f->check(_log, _verbose, _stream, _tabs + "  ");
     return _log.is_consistent_;
 }
-} // namespace wolf
+}  // namespace wolf

src/capture/capture_diff_drive.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "core/capture/capture_diff_drive.h"
 #include "core/math/rotations.h"
 
-namespace wolf {
-
-CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
-                                   const SensorBasePtr& _sensor_ptr,
+namespace wolf
+{
+CaptureDiffDrive::CaptureDiffDrive(const TimeStamp&       _ts,
+                                   const SensorBasePtr&   _sensor_ptr,
                                    const Eigen::VectorXd& _data,
                                    const Eigen::MatrixXd& _data_cov,
-                                   CaptureBasePtr _capture_origin_ptr,
-                                   StateBlockPtr _p_ptr,
-                                   StateBlockPtr _o_ptr,
-                                   StateBlockPtr _intr_ptr) :
-  CaptureMotion("CaptureDiffDrive",
-                _ts,
-                _sensor_ptr,
-                _data,
-                _data_cov,
-                _capture_origin_ptr,
-                _p_ptr,
-                _o_ptr,
-                _intr_ptr)
+                                   CaptureBasePtr         _capture_origin_ptr,
+                                   const VectorComposite& _state_vectors,
+                                   const PriorComposite&  _state_priors)
+    : CaptureMotion("CaptureDiffDrive",
+                    _ts,
+                    _sensor_ptr,
+                    _data,
+                    _data_cov,
+                    _capture_origin_ptr,
+                    {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'I', "StateParams3"}},
+                    _state_vectors,
+                    _state_priors)
 {
     //
 }
 
-} // namespace wolf
+}  // namespace wolf

src/capture/capture_landmarks_external.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "core/capture/capture_landmarks_external.h"
 
-namespace wolf{
-
-CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts, 
-                                                   SensorBasePtr _sensor_ptr, 
-                                                   const std::vector<int>& _ids,
-                                                   const std::vector<int>& _types,
-                                                   const std::vector<Eigen::VectorXd>& _detections, 
-                                                   const std::vector<Eigen::MatrixXd>& _covs, 
-                                                   const std::vector<double>& _qualities) :
-	CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
+namespace wolf
 {
-    if (_ids.size() != _detections.size() or 
-        _ids.size() != _covs.size() or 
-        _ids.size() != _qualities.size())
-        throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
-    
+CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp&                    _ts,
+                                                   SensorBasePtr                       _sensor_ptr,
+                                                   const std::vector<int>&             _ids,
+                                                   const std::vector<int>&             _types,
+                                                   const std::vector<Eigen::VectorXd>& _detections,
+                                                   const std::vector<Eigen::MatrixXd>& _covs,
+                                                   const std::vector<double>&          _qualities)
+    : CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
+{
+    if (_ids.size() != _detections.size() or _ids.size() != _covs.size() or _ids.size() != _qualities.size())
+        throw std::runtime_error(
+            "CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same "
+            "size.");
+
     for (auto i = 0; i < _detections.size(); i++)
         addDetection(_ids.at(i), _types.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
 }
 
 CaptureLandmarksExternal::~CaptureLandmarksExternal()
 {
-	//
+    //
 }
 
-void CaptureLandmarksExternal::addDetection(const int& _id, const int& _type, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
+void CaptureLandmarksExternal::addDetection(const int&             _id,
+                                            const int&             _type,
+                                            const Eigen::VectorXd& _detection,
+                                            const Eigen::MatrixXd& _cov,
+                                            const double&          _quality)
 {
     detections_.push_back(LandmarkDetection{_id, _type, _detection, _cov, _quality});
 }
 
-} // namespace wolf
+}  // namespace wolf

src/capture/capture_motion.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/**
- * \file capture_motion.cpp
- *
- *  Created on: Oct 14, 2017
- *      \author: jsola
- */
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "core/capture/capture_motion.h"
 
 namespace wolf
 {
-
-CaptureMotion::CaptureMotion(const std::string & _type,
-                             const TimeStamp& _ts,
-                             SensorBasePtr _sensor_ptr,
+CaptureMotion::CaptureMotion(const std::string&     _type,
+                             const TimeStamp&       _ts,
+                             SensorBasePtr          _sensor_ptr,
                              const Eigen::VectorXd& _data,
-                             CaptureBasePtr _capture_origin_ptr) :
-        CaptureBase(_type, _ts, _sensor_ptr),
-        data_(_data),
-        data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXd::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
-        buffer_(),
-        capture_origin_ptr_(_capture_origin_ptr)
+                             CaptureBasePtr         _capture_origin_ptr,
+                             const TypeComposite&   _state_types,
+                             const VectorComposite& _state_vectors,
+                             const PriorComposite&  _state_priors)
+    : CaptureBase(_type, _ts, _sensor_ptr, _state_types, _state_vectors, _state_priors),
+      data_(_data),
+      data_cov_(_sensor_ptr ? _sensor_ptr->computeNoiseCov(_data)
+                            : Eigen::MatrixXd::Zero(
+                                  _data.rows(),
+                                  _data.rows())),  // Someone should test this zero and do something smart accordingly
+      buffer_(),
+      capture_origin_ptr_(_capture_origin_ptr)
 {
-    //
 }
 
-CaptureMotion::CaptureMotion(const std::string & _type,
-                             const TimeStamp& _ts,
-                             SensorBasePtr _sensor_ptr,
+CaptureMotion::CaptureMotion(const std::string&     _type,
+                             const TimeStamp&       _ts,
+                             SensorBasePtr          _sensor_ptr,
                              const Eigen::VectorXd& _data,
                              const Eigen::MatrixXd& _data_cov,
-                             CaptureBasePtr _capture_origin_ptr,
-                             StateBlockPtr _p_ptr ,
-                             StateBlockPtr _o_ptr ,
-                             StateBlockPtr _intr_ptr ) :
-                CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
-                data_(_data),
-                data_cov_(_data_cov),
-                buffer_(),
-                capture_origin_ptr_(_capture_origin_ptr)
+                             CaptureBasePtr         _capture_origin_ptr,
+                             const TypeComposite&   _state_types,
+                             const VectorComposite& _state_vectors,
+                             const PriorComposite&  _state_priors)
+    : CaptureBase(_type, _ts, _sensor_ptr, _state_types, _state_vectors, _state_priors),
+      data_(_data),
+      data_cov_(_data_cov),
+      buffer_(),
+      capture_origin_ptr_(_capture_origin_ptr)
 {
-    //
 }
 
 CaptureMotion::~CaptureMotion()
@@ -68,17 +61,19 @@ CaptureMotion::~CaptureMotion()
     //
 }
 
-bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) const
+bool CaptureMotion::containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const
 {
     assert(_ts.ok() and this->time_stamp_.ok());
 
-    // the same capture is within tolerance
-    if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance)
-        return true;
-
-    // buffer is empty, and the capture is not within tolerance
+    // if buffer is empty, check if the capture is within tolerance
     if (this->getBuffer().empty())
-        return false;
+    {
+        if (this->time_stamp_ - _time_tolerance <= _ts and _ts <= this->time_stamp_ + _time_tolerance)
+            return true;
+        else
+            // buffer is empty (and the capture is not within tolerance)
+            return false;
+    }
 
     // buffer encloses timestamp, if ts is:
     //   from : buffer.first.ts - tt
@@ -91,16 +86,16 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera
     return false;
 }
 
-
-
-
-void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void CaptureMotion::printHeader(int           _depth,
+                                bool          _factored_by,
+                                bool          _metric,
+                                bool          _state_blocks,
+                                std::ostream& _stream,
+                                std::string   _tabs) const
 {
-    _stream << _tabs << "CapM" << id()
-            << " " << getType()
-            << " ts = " << std::setprecision(3) << getTimeStamp();
+    _stream << _tabs << "CapM" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
 
-    if(getSensor() != nullptr)
+    if (getSensor() != nullptr)
     {
         _stream << " -> Sen" << getSensor()->id();
     }
@@ -110,37 +105,37 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
     if (auto OC = getOriginCapture())
     {
         _stream << " -> Origin: Cap" << OC->id();
-        if (auto OF = OC->getFrame())
-            _stream << " ; Frm" << OF->id();
+        if (auto OF = OC->getFrame()) _stream << " ; Frm" << OF->id();
     }
 
     _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
-    if (_constr_by)
+    if (_factored_by)
     {
         _stream << "  <-- ";
-        for (auto cby : getConstrainedByList())
-            _stream << "Fac" << cby->id() << " \t";
+        for (auto fac : getFactoredBySet())
+            if (fac) _stream << "Fac" << fac->id() << " \t";
     }
     _stream << std::endl;
 
-    if (getStateBlockMap().size() > 0)
-        printState(_metric, _state_blocks, _stream, _tabs);
+    if (getStateBlockMap().size() > 0) printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
 
-    _stream << _tabs << "  " << "buffer size  :  " << getBuffer().size();
+    _stream << _tabs << "  "
+            << "buffer size  :  " << getBuffer().size();
     if (getBuffer().size() > 0) _stream << " ; nbr of data samples : " << getBuffer().size() - 1;
     _stream << std::endl;
 
-    if ( _metric && ! getBuffer().empty())
+    if (_metric && !getBuffer().empty())
     {
-        _stream << _tabs << "  " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
-//        if (hasCalibration())
-//        {
-//            _stream << _tabs << "  " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
-//            _stream << _tabs << "  " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
-//            _stream << _tabs << "  " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
-//            _stream << _tabs << "  " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
-//        }
+        _stream << _tabs << "  "
+                << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
+        //        if (hasCalibration())
+        //        {
+        //            _stream << _tabs << "  " << "calib preint : (" << getCalibrationPreint().transpose() << ")" <<
+        //            std::endl; _stream << _tabs << "  " << "jacob preint : (" << getJacobianCalib().row(0) << ")" <<
+        //            std::endl; _stream << _tabs << "  " << "calib current: (" << getCalibration().transpose() << ")"
+        //            << std::endl; _stream << _tabs << "  " << "delta correct: (" <<
+        //            getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+        //        }
     }
-
-}
 }
+}  // namespace wolf

src/capture/capture_odom_2d.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * capture_odom_2d.cpp
- *
- *  Created on: Oct 16, 2017
- *      Author: jsola
- */
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "core/capture/capture_odom_2d.h"
 
 namespace wolf
 {
-
-CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
-                             SensorBasePtr _sensor_ptr,
+CaptureOdom2d::CaptureOdom2d(const TimeStamp&       _init_ts,
+                             SensorBasePtr          _sensor_ptr,
                              const Eigen::VectorXd& _data,
-                             CaptureBasePtr _capture_origin_ptr):
-        CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
+                             CaptureBasePtr         _capture_origin_ptr)
+    : CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
 {
     //
 }
 
-CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
-                             SensorBasePtr _sensor_ptr,
+CaptureOdom2d::CaptureOdom2d(const TimeStamp&       _init_ts,
+                             SensorBasePtr          _sensor_ptr,
                              const Eigen::VectorXd& _data,
                              const Eigen::MatrixXd& _data_cov,
-                             CaptureBasePtr _capture_origin_ptr):
-        CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
+                             CaptureBasePtr         _capture_origin_ptr)
+    : CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
 {
     //
 }

src/capture/capture_odom_3d.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * capture_odom_3d.cpp
- *
- *  Created on: Oct 16, 2017
- *      Author: jsola
- */
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "core/capture/capture_odom_3d.h"
 
 namespace wolf
 {
-
-CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
-                             SensorBasePtr _sensor_ptr,
+CaptureOdom3d::CaptureOdom3d(const TimeStamp&       _init_ts,
+                             SensorBasePtr          _sensor_ptr,
                              const Eigen::VectorXd& _data,
-                             CaptureBasePtr _capture_origin_ptr):
-        CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
+                             CaptureBasePtr         _capture_origin_ptr)
+    : CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
 {
     //
 }
 
-CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
-                             SensorBasePtr _sensor_ptr,
+CaptureOdom3d::CaptureOdom3d(const TimeStamp&       _init_ts,
+                             SensorBasePtr          _sensor_ptr,
                              const Eigen::VectorXd& _data,
                              const Eigen::MatrixXd& _data_cov,
-                             CaptureBasePtr _capture_origin_ptr):
-        CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
+                             CaptureBasePtr         _capture_origin_ptr)
+    : CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
 {
     //
 }
@@ -56,4 +45,3 @@ CaptureOdom3d::~CaptureOdom3d()
 }
 
 } /* namespace wolf */
-