Merge pull request #9 from tipf/master

Update Docs & Matlab

README.md

@@ -119,7 +119,7 @@ The following pages give you an overview, how to use them or how to build a cust
 
 2. [How to use the robust Gaussian mixture models from our RA-L 2021 Paper?](docs/ROBUST.md)
 
-3. [How to build your own application on top of the libRSF? (under construction)](docs/CUSTOM.md)
+3. [How to build your own application on top of the libRSF?](docs/CUSTOM.md)
 
 ## Additional Information
 
@@ -141,8 +141,8 @@ This library also contains the implementation of [1-3]. Further references will
 
 [2] *Tim Pfeifer and Peter Protzel*, Incrementally learned Mixture Models for GNSS Localization, Proc. of Intelligent Vehicles Symposium (IV), 2019, DOI: [10.1109/IVS.2019.8813847](https://doi.org/10.1109/IVS.2019.8813847)
 
-[3] *Tim Pfeifer and Sven Lange and Peter Protzel*, Advancing Mixture Models for Least Squares Optimization, Robotics and Automation Letters (RA-L), 2021 (accepted), Preprint: [arXiv:2103.02472](https://arxiv.org/abs/2103.02472)
+[3] *Tim Pfeifer and Sven Lange and Peter Protzel*, Advancing Mixture Models for Least Squares Optimization, Robotics and Automation Letters (RA-L), 2021, DOI: [10.1109/LRA.2021.3067307](https://dx.doi.org/10.1109/LRA.2021.3067307)
 
 ### License
 
-This work is released under the GNU General Public License version 3.
+This work is released under the GNU General Public License version 3.

docs/CUSTOM_FACTORS.md

@@ -44,11 +44,11 @@ If you want to use our factors for your code, you have two options:
 
 1. You use them with our `FactorGraph` class that has an dedicated `addFactor()` interface.  For examples, just have a look to the examples in the "applications" folder.
 
-2. If you want to use them in a vanilla Ceres application you can create a cost function object with the following code snippet (example for a 2D range measurement). If you need more information about the required `SensorData`class, have a look at our [data structures](CUSTOM_IN_OUT.md).
+2. If you want to use them in a vanilla Ceres application you can create a cost function object with the following code snippet (example for a 2D range measurement). If you need more information about the required `Data`class, have a look at our [data structures](CUSTOM_IN_OUT.md).
 
    ```c++
    /** get a range measurement from somewhere */
-   libRSF::SensorData RangeMeasurement = ...
+   libRSF::Data RangeMeasurement = ...
 
    /** create error model */
    typedef libRSF::GaussianDiagonal<1> ErrorClass;
@@ -63,4 +63,4 @@ If you want to use our factors for your code, you have two options:
    auto CostFunction = new ceres::AutoDiffCostFunction<FactorClass, ErrorClass::OutputDim, 2> (Factor);
    ```
 
-
+

docs/CUSTOM_IN_OUT.md

@@ -1,24 +1,33 @@
 ## One class to store them all
 
--  purpose
-   -  store (relativly) big a mount of heterogenous data
-- general concept of data
-  - a map of elementIDs and vectors
-  - acces through setter and getters
-  - config define the structure --> point to config
-- general concept of dataset
-  - multimaps inside a map
-  - first layer is ID
-  - second layer is time
-  - double for time is dangerous, keep in mind
+To store all the data that accumulates from a variety of sensors, we created the `Data` class which is located in the [Data.h](../include/Data.h) header file.
+It holds an map of (dynamically sized) vectors that store the sub-elements of a measurement. For example its mean and its variance.
+The map is indexed by the `DataElement` enum, which you can find in the [Types.h](../include/Types.h) header file.
 
-### Measurement Data
+For each "kind" of measurement, there is a configuration object that initialize a specific data type. These configuration are stored in the [Types.cpp](../src/Types.cpp).
+There is another enum `DataType`, which connects a Data object with it's configuration.
 
-* store measurements
-* type as ID
+The `Data` class is also used to store the state variables
 
-### State Data
+### Streams of Data
 
-* store optimized values for ceres pointer interface
-* different states with the same type --> string as ID
+A `DataStream` is a chronological ordered multimap of `Data` objects. **Multi**map, because there can be multiple entries fur the same timestamp.
+We use double timestamps as keys for this multimap, but round them internally to 1e-3 precision to prevent problems with floating point comparisons.
 
+The `DataStream` class can be found in [DataStream.h](../include/DataStream.h).
+
+### Sets of Data
+
+Multiple `DataStream` objects can be stored in an `DataSet`. Our implementation of this `DataSet` class is again a map that contains indexed `DataStream` objects. Each stream has an unique key, but the type of the key depends on the use-case.
+
+#### SensorDataSet
+
+For measurements, we use `DataType` enums as Keys because we assume that each sensor is unique and connected to a specific type of data. The resulting `SensorDataSet` can be found in the [SensorDataSet.h](../include/SensorDataSet.h) header.
+
+#### StateDataSet
+
+For state variables of the factor graph, there is a separate `StateDataSet` class in the [StateDataSet.h](../include/StateDataSet.h) header.
+Here, we use strings as keys to allow more flexibility.
+
+The `StateDataSet` is usually used inside our `FactorGraph` class and internally connected to the Ceres back-end by raw pointers.
+We created it as a more convenient interface for the optimized variables.

docs/ROBUST.md

@@ -166,7 +166,7 @@ The point set registration example from [1] is currently not available for the C
 
 If you are interested in this work, feel free to have a look at our recent paper:
 
-[1] *Tim Pfeifer and Sven Lange and Peter Protzel*, Advancing Mixture Models for Least Squares Optimization, Robotics and Automation Letters (RA-L), 2021
+[1] *Tim Pfeifer and Sven Lange and Peter Protzel*, Advancing Mixture Models for Least Squares Optimization, Robotics and Automation Letters (RA-L), 2021, DOI: [10.1109/LRA.2021.3067307](https://dx.doi.org/10.1109/LRA.2021.3067307)
 
 BibTeX:
 
@@ -187,4 +187,4 @@ Our approach is strongly inspired by the former work of Olsen and Rosen, therefo
 robust robot mapping, Int. Journal of Robotics Research, 2013
 
 [3] *David M. Rosen and Michael Kaess and John J. Leonard*, Robust Incremental Online Inference Over Sparse
-Factor Graphs: Beyond the Gaussian Case, Proc. of Int. Conf. on Robotics and Automation (ICRA), 2013
+Factor Graphs: Beyond the Gaussian Case, Proc. of Int. Conf. on Robotics and Automation (ICRA), 2013

examples/Example_FG_Generic.cpp

@@ -41,7 +41,7 @@ int main(int ArgC, char** ArgV)
   /** create our own graph object */
   libRSF::FactorGraph SimpleGraph;
 
-  /** set the solver options for ceres */
+  /** set the solver options for Ceres */
   ceres::Solver::Options SolverOptions;
   SolverOptions.trust_region_strategy_type = ceres::TrustRegionStrategyType::DOGLEG;
   SolverOptions.dogleg_type = ceres::DoglegType::SUBSPACE_DOGLEG;
@@ -113,4 +113,4 @@ int main(int ArgC, char** ArgV)
   return 0;
 }
 
-#endif // TESTMODE
+#endif // TESTMODE

matlab/+libRSF/translateToYaml.m

@@ -49,9 +49,9 @@
                     case 'gnss'
                         Lables{end} = [Lables{end} 'GNSS'];
                     case 'cced'
-                        do nothing
+                        %do nothing
                     case 'cce'
-                        do nothing
+                        %do nothing
                     case 'loop'
                         Lables{end} = [Lables{end} 'Loop Closure 2D'];
                     case 'radar'

matlab/+libRSF/wrapCeres.m

@@ -102,8 +102,8 @@
 Result.Runtime = Runtime;
 
 %% save time difference
-if isfield(Data, 'StartTime')
-    Result.StartTime = Data.StartTime;
+if isfield(Data, 'Info')
+    Result.StartTime = Data.Info.StartTime;
 else
     Result.StartTime = 0;
 end

matlab/Estimation/Robust_Models/PlotRobust1D.m

@@ -22,7 +22,7 @@
 hCost = figure;
 subplot(3,1,1)
 for n = 1:numel(ErrorModels)
-    hLine = plot(Result.Error(:,n), Result.Cost(:,n) - min(Result.Cost(:,n)), 'LineWidth', Config.Line.Width, 'Color', Config.Line.Color(n,:));
+    hLine = plot(Result.Error(:,n), Result.Cost(:,n) - min(Result.Cost(:,n)), 'LineWidth', Config.Line.Width, 'Color', Config.Color.Default(n,:));
     set(hLine, 'DisplayName', ErrorModels{n});
     hold on
 end
@@ -36,7 +36,7 @@
 % gradient
 subplot(3,1,2)
 for n = 1:numel(ErrorModels)
-    h = plot(Result.Error(:,n), Result.Gradient(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Line.Color(n,:));
+    h = plot(Result.Error(:,n), Result.Gradient(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Color.Default(n,:));
     set(h, 'DisplayName', ErrorModels{n});
     hold on
 end
@@ -50,7 +50,7 @@
 % Hessian
 subplot(3,1,3)
 for n = 1:numel(ErrorModels)
-    h = plot(Result.Error(:,n), Result.Hessian(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Line.Color(n,:));
+    h = plot(Result.Error(:,n), Result.Hessian(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Color.Default(n,:));
     set(h, 'DisplayName', ErrorModels{n});
     hold on
 end
@@ -72,7 +72,7 @@
 %% plot probability
 hProb = figure;
 for n = 1:numel(ErrorModels)
-    hLine = plot(Result.Error(:,n), Result.Prob(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Line.Color(n,:));
+    hLine = plot(Result.Error(:,n), Result.Prob(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Color.Default(n,:));
     set(hLine,'DisplayName',ErrorModels{n});
     hold on
 end
@@ -88,9 +88,9 @@
 hOpt = figure;
 hold on
 for n = 1:numel(ErrorModels)
-    hLine = plot(Result.Error(:,n), Result.Cost(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Line.Color(n,:));
+    hLine = plot(Result.Error(:,n), Result.Cost(:,n), 'LineWidth', Config.Line.Width, 'Color', Config.Color.Default(n,:));
     set(hLine,'DisplayName',ErrorModels{n});
-    plot(-Result.PostOpt(:,n), ones(size(Result.PostOpt(:,n)))*n + min(Result.Cost,[],'all'), 'x', 'MarkerSize', Config.Line.Marker.Size, 'LineWidth', Config.Line.Width, 'Color', Config.Line.Color(n,:), 'HandleVisibility','off')
+    plot(-Result.PostOpt(:,n), ones(size(Result.PostOpt(:,n)))*n + min(Result.Cost,[],'all'), 'x', 'MarkerSize', Config.Line.Marker.Size, 'LineWidth', Config.Line.Width, 'Color', Config.Color.Default(n,:), 'HandleVisibility','off')
 end
 hold off
 legend('show');
@@ -109,7 +109,7 @@
     hHist.Normalization = 'pdf';
     hHist.BinEdges = hHist.BinEdges - hHist.BinWidth/2.0;
     hHist.LineStyle = 'none';
-    hHist.FaceColor = Config.Line.Color(n,:);
+    hHist.FaceColor = Config.Color.Default(n,:);
 end
 hold off
 % Format
@@ -130,7 +130,7 @@
     hHist.Normalization = 'pdf';
     hHist.BinEdges = hHist.BinEdges - hHist.BinWidth/2.0;
     hHist.LineStyle = 'none';
-    hHist.FaceColor = Config.Line.Color(n,:);
+    hHist.FaceColor = Config.Color.Default(n,:);
 end
 hold off
 % Format

matlab/Estimation/Robust_Models/PlotRobust2D.m

@@ -81,7 +81,7 @@
     hScatter{n} = plot(-Result.PostOpt(InBound,1,n), -Result.PostOpt(InBound,2,n),'o', 'LineWidth', 1.25);
     hScatter{n}.MarkerSize = Config.Line.Marker.Size;
     hScatter{n}.MarkerEdgeColor = 'k';
-    hScatter{n}.MarkerFaceColor = Config.Line.Color(n,:);
+    hScatter{n}.MarkerFaceColor = Config.Color.Default(n,:);
 
     set(hScatter{n}, 'DisplayName', Lables(n));
 end
@@ -121,7 +121,7 @@
 nexttile([1 2])
 hold on
 for n = 1:NumberAlgo
-    hCostSlice{n} = plot(LineX, LineCostAlgo(:,n),'-', 'LineWidth', Config.Line.Width, 'Color', Config.Line.Color(n,:));
+    hCostSlice{n} = plot(LineX, LineCostAlgo(:,n),'-', 'LineWidth', Config.Line.Width, 'Color', Config.Color.Default(n,:));
     hCostSlice{n}.DisplayName = Lables(n);
 end
 hold off
@@ -158,7 +158,7 @@
     hHist.Normalization = 'pdf';
     hHist.BinEdges = hHist.BinEdges - hHist.BinWidth/2.0;
     hHist.LineStyle = 'none';
-    hHist.FaceColor = Config.Line.Color(n,:);
+    hHist.FaceColor = Config.Color.Default(n,:);
 end
 hold off
 % Format
@@ -179,7 +179,7 @@
     hHist.Normalization = 'pdf';
     hHist.BinEdges = hHist.BinEdges - hHist.BinWidth/2.0;
     hHist.LineStyle = 'none';
-    hHist.FaceColor = Config.Line.Color(n,:);
+    hHist.FaceColor = Config.Color.Default(n,:);
 end
 hold off
 % Format
@@ -215,7 +215,7 @@
 %     hold on
 %     quiver(ErrorX, ErrorY, -GradientX, -GradientY);
 %     quiver(ErrorX, ErrorY, DY, DX);
-%     plot(Result.PostOpt(1,1,n), Result.PostOpt(1,2,n),'x', 'MarkerSize', Config.Line.Marker.Size, 'LineWidth', Config.Line.Width*2, 'Color', Config.Line.Color(1,:), 'HandleVisibility','off');
+%     plot(Result.PostOpt(1,1,n), Result.PostOpt(1,2,n),'x', 'MarkerSize', Config.Line.Marker.Size, 'LineWidth', Config.Line.Width*2, 'Color', Config.Color.Default(1,:), 'HandleVisibility','off');
 %     hold off
 %     
 %     legend({['Cost - ' ErrorModels{n}], ['Gradient - ' ErrorModels{n}], ['Gradient Numerical  - ' ErrorModels{n}]});

matlab/Estimation/Robust_Models/PlotRobustCombined.m

@@ -23,7 +23,7 @@
     hHist.Normalization = 'pdf';
     hHist.BinEdges = hHist.BinEdges - hHist.BinWidth/2.0;
     hHist.LineStyle = 'none';
-    hHist.FaceColor = Config.Line.Color(n,:);
+    hHist.FaceColor = Config.Color.Default(n,:);
 end
 hold off
 
@@ -52,7 +52,7 @@
     hHist.Normalization = 'pdf';
     hHist.BinEdges = hHist.BinEdges - hHist.BinWidth/2.0;
     hHist.LineStyle = 'none';
-    hHist.FaceColor = Config.Line.Color(n,:);
+    hHist.FaceColor = Config.Color.Default(n,:);
 end
 hold off
 

matlab/Externals/utility/+plot/createAreaPlotGeneric.m

@@ -11,8 +11,8 @@
 
 p = area(XData,YData);
 for nLable =1:length(Lables)
-    p(nLable).EdgeColor = Config.Line.Color(nLable,:);
-    p(nLable).FaceColor = Config.Line.Color(nLable,:);
+    p(nLable).EdgeColor = Config.Color.Default(nLable,:);
+    p(nLable).FaceColor = Config.Color.Default(nLable,:);
     p(nLable).DisplayName = Lables{nLable};
 end
 

matlab/Externals/utility/+plot/createBoxPlot.m

@@ -29,12 +29,12 @@
 
 
 %% set colors
-set(findobj(hBox,'tag','Median'), 'Color', Config.Line.Color(1,:));
-set(findobj(hBox,'tag','Box'), 'Color', Config.Line.Color(3,:));
+set(findobj(hBox,'tag','Median'), 'Color', Config.Color.Default(1,:));
+set(findobj(hBox,'tag','Box'), 'Color', Config.Color.Default(3,:));
 
 h = findobj(hBox,'tag','Outliers');
 for iH = 1:length(h)
-    h(iH).MarkerEdgeColor = Config.Line.Color(1,:);
+    h(iH).MarkerEdgeColor = Config.Color.Default(1,:);
 end
 
 end

matlab/Externals/utility/+plot/createPlot2D.m

@@ -61,7 +61,7 @@
 
 % plot trajectories
 for m = 1:M
-    hPlotTraj = plot(Trajectory(m,:,1),Trajectory(m,:,2),':','LineWidth',PlotConfig.Line.Width,'MarkerSize',15, 'Color',PlotConfig.Line.Color(m,:));
+    hPlotTraj = plot(Trajectory(m,:,1),Trajectory(m,:,2),':','LineWidth',PlotConfig.Line.Width,'MarkerSize',15, 'Color',PlotConfig.Color.Default(m,:));
     hPlotTraj.DisplayName = Metric(m).Lable;
 end
 
@@ -92,6 +92,16 @@
 grid on
 box on
 
+% set boundary around GT
+MinX = min(GT(:,1));
+MaxX = max(GT(:,1));
+MinY = min(GT(:,2));
+MaxY = max(GT(:,2));
+RangeX = MaxX - MinX;
+RangeY = MaxY - MinY;
+xlim([MinX MaxX] + [-RangeX RangeX]*0.1 + [-10 10]);
+ylim([MinY MaxY] + [-RangeY RangeY]*0.1 + [-10 10]);
+
 % find equal ticks
 XT = xticks;
 YT = yticks;
@@ -102,8 +112,7 @@
 yticks(unique(round(YT/ResMax))*ResMax)
 
 % add legend
-legend('show')
-legend('Location','best')
+legend('show', 'Location', 'best');
 
 %% Boxplot
 BoxConfig = PlotConfig;
@@ -212,7 +221,7 @@
         hold on
         % plot trajectories
         for m = 1:M
-            hPlotPints{idx,m} = plot(hAxe, Trajectory(m,idx*StepSize,1),Trajectory(m,idx*StepSize,2),'.','LineWidth',PlotConfig.Line.Width,'MarkerSize',15, 'Color',PlotConfig.Line.Color(m,:));
+            hPlotPints{idx,m} = plot(hAxe, Trajectory(m,idx*StepSize,1),Trajectory(m,idx*StepSize,2),'.','LineWidth',PlotConfig.Line.Width,'MarkerSize',15, 'Color',PlotConfig.Color.Default(m,:));
             hPlotPints{idx,m}.DisplayName = Metric(m).Lable;
         end
 

matlab/Externals/utility/+plot/createPlotGeneric.m

@@ -10,7 +10,7 @@
 hold on
 
 for nLable =1:length(Lables)
-    p = plot(XData(nLable,:), YData(nLable,:),'LineWidth',Config.Line.Width,'Color',Config.Line.Color(nLable,:));
+    p = plot(XData(nLable,:), YData(nLable,:),'LineWidth',Config.Line.Width,'Color',Config.Color.Default(nLable,:));
     p.DisplayName = Lables{nLable};
 end
 

matlab/Externals/utility/+plot/exportPlot.m

@@ -3,14 +3,22 @@
     drawnow update
     pause(0.1);
 
+    % convert to string
+    if ischar(Path)
+        Path  = convertCharsToStrings(Path);
+    end
+    if ischar(Name)
+        Name  = convertCharsToStrings(Name);
+    end
+
     % concat filname
-    FullFile = [Path Name];
-    FullFilePDF = [FullFile '.pdf'];
-    FullFilePNG = [FullFile '.png'];
-    FullFileFig = [FullFile '.fig'];
+    FullFile = Path + Name;
+    FullFilePDF = FullFile + ".pdf";
+    FullFilePNG = FullFile + ".png";
+    FullFileFig = FullFile + ".fig";
 
     % save as figure file
-    savefig(Handle,FullFileFig,'compact');
+    savefig(Handle, FullFileFig, 'compact');
 
     % save as graphics file
     if exist('exportgraphics', 'file')