diff --git a/README.md b/README.md
index 9949508..20449ce 100644
--- a/README.md
+++ b/README.md
@@ -13,7 +13,7 @@ Main features are:
This work is released under the GNU General Public License version 3.
### Citation
-This library is the implementation of the following paper [1]. Further references will be added with additional content.
+This library is the implementation of [1] and [2]. Further references will be added with additional content.
[1] Tim Pfeifer and Peter Protzel, Expectation-Maximization for Adaptive Mixture Models in Graph Optimization, Proc. of Intl. Conf. on Robotics and Automation (ICRA), 2019
@@ -26,6 +26,17 @@ BibTeX:
year = {2019},
}
+[2] Tim Pfeifer and Peter Protzel, Incrementally learned Mixture Models for GNSS Localization, Proc. of Intelligent Vehicles Symposium (IV), 2019
+
+BibTeX:
+
+ @InProceedings{Pfeifer2019a,
+ author = {Tim Pfeifer and Peter Protzel},
+ title = {Incrementally learned Mixture Models for GNSS Localization},
+ booktitle = {Proc. of Intelligent Vehicles Symposium (IV)},
+ year = {2019},
+ }
+
## Installation
The libRSF is a CMake project that requires the installation of the following dependencies:
@@ -64,7 +75,7 @@ After building the library, some applications are provided. Usually they corresp
#### ICRA 2019
These two applications are made for the ICRA 2019 conference, the corresponding paper is [1].
-One can be used for GNSS datasets and calculated a 3D position in the ECEF frame, while the other one is for the 2D UWB ranging dataset.
+One can be used for GNSS datasets and calculates a 3D position in the ECEF frame, while the other one is for 2D ranging datasets.
To run them, the following syntax have to be used:
libRSF/build/applications/ICRA19_GNSS error:
@@ -99,3 +110,36 @@ To run them, the following syntax have to be used:
A full example could be:
libRSF/build/applications/ICRA19_GNSS libRSF/datasets/smartLoc/Data_Berlin_Potsdamer_Platz_Web.txt Result_Berlin_Potsdamer_Platz_Web.txt error: gauss
+
+#### IV 2019
+
+These application is made for the IV 2019 conference, the corresponding paper is [2].
+It can be used for GNSS datasets and calculates a 3D position in the ECEF frame.
+To run them, the following syntax have to be used:
+
+ libRSF/build/applications/IV19_GNSS error:
+
+- **\** is the estimated Trajectory. The output file contains several columns that represent timestamps and estimated positions:
+
+ Column 1 - Timestamp [s]
+ Column 2 - X coordinate in the ECEF frame [m]
+ Column 3 - Y coordinate in the ECEF frame [m]
+ Column 4 - Z coordinate in the ECEF frame [m]
+ Column 5-13 - Covariance matrix of the estimated position in row-major format (Currently not used!)
+
+- **\** is one of the following error models:
+
+ gauss - A Gaussian distribution
+ dcs - Dynamic Covariance Scaling
+ cdce - Closed form Dynamic Covariance Estimation
+ mm - Max-Mixture (an approximation of a Gaussian mixture)
+ sm - Sum-Mixture (an exact Gaussian mixture)
+ stmm - Adaptive Max-Mixture using the EM Algorithm
+ stsm - Adaptive Sum-Mixture using the EM Algorithm
+ stmm_vbi - Incrementally learned Max-Mixture using the VBI Algorithm
+ stsm_vbi - Incrementally learned Sum-Mixture using the VIB Algorithm
+
+A full example could be:
+
+ libRSF/build/applications/IV19_GNSS libRSF/datasets/smartLoc/Data_Berlin_Potsdamer_Platz_Web.txt Result_Berlin_Potsdamer_Platz_Web.txt error: gauss
diff --git a/applications/CMakeLists.txt b/applications/CMakeLists.txt
index 91b6afb..b05ee45 100644
--- a/applications/CMakeLists.txt
+++ b/applications/CMakeLists.txt
@@ -23,3 +23,6 @@ target_link_libraries(ICRA19_Ranging ${CERES_LIBRARIES} libRSF)
add_executable(ICRA19_GNSS ICRA19_GNSS.cpp)
target_link_libraries(ICRA19_GNSS ${CERES_LIBRARIES} libRSF)
+
+add_executable(IV19_GNSS IV19_GNSS.cpp)
+target_link_libraries(IV19_GNSS ${CERES_LIBRARIES} libRSF)
diff --git a/applications/ICRA19_GNSS.cpp b/applications/ICRA19_GNSS.cpp
index b1b28b4..b7df00f 100644
--- a/applications/ICRA19_GNSS.cpp
+++ b/applications/ICRA19_GNSS.cpp
@@ -205,7 +205,7 @@ void TuneErrorModel(libRSF::FactorGraph &Graph,
for(int nComponent = 0; nComponent < NumberOfComponents; ++nComponent)
{
- Component.setParamsStdDev((ceres::Vector(1) << 10).finished()*pow(10, nComponent),
+ Component.setParamsStdDev((ceres::Vector(1) << pow(10, nComponent+1)).finished(),
(ceres::Vector(1) << 0.0).finished(),
(ceres::Vector(1) << 1.0/NumberOfComponents).finished());
diff --git a/applications/ICRA19_Ranging.cpp b/applications/ICRA19_Ranging.cpp
index e4f8961..ca02bcb 100644
--- a/applications/ICRA19_Ranging.cpp
+++ b/applications/ICRA19_Ranging.cpp
@@ -31,7 +31,6 @@
#include "ICRA19_Ranging.h"
-
/** @brief Generates a delta time measurement object from two timestamps
*
* @param TimestampOld a double timestamp
@@ -310,8 +309,6 @@ int main(int argc, char** argv)
Graph.addState(ORIENTATION_STATE, libRSF::StateType::Angle, Timestamp);
/** add motion model or odometry */
- libRSF::SensorData DeltaTime = GenerateDeltaTime(TimestampOld, Timestamp);
-
libRSF::StateList MotionList;
MotionList.add(POSITION_STATE, TimestampOld);
MotionList.add(POSITION_STATE, Timestamp);
diff --git a/applications/IV19_GNSS.cpp b/applications/IV19_GNSS.cpp
new file mode 100644
index 0000000..75aa63b
--- /dev/null
+++ b/applications/IV19_GNSS.cpp
@@ -0,0 +1,469 @@
+/***************************************************************************
+ * libRSF - A Robust Sensor Fusion Library
+ *
+ * Copyright (C) 2018 Chair of Automation Technology / TU Chemnitz
+ * For more information see https://www.tu-chemnitz.de/etit/proaut/self-tuning
+ *
+ * libRSF is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * libRSF 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with libRSF. If not, see ::Ones());
+ Graph.addFactor(libRSF::FactorType::Pseudorange3, ListPseudorange, Pseudorange, NoisePseudorange, new libRSF::cDCELoss(Pseudorange.getStdDev()[0]));
+ break;
+
+ case ROBUST_MM:
+ static libRSF::GaussianMixture<1> GMM_MM;
+ if (GMM_MM.getNumberOfComponents() == 0)
+ {
+ InitGMM(GMM_MM, GMM_N, 10);
+ }
+ static libRSF::MaxMix1 NoisePseudorangeMaxMix(GMM_MM);
+
+ Graph.addFactor(libRSF::FactorType::Pseudorange3, ListPseudorange, Pseudorange, NoisePseudorangeMaxMix);
+
+ break;
+
+ case ROBUST_SM:
+ static libRSF::GaussianMixture<1> GMM_SM;
+ if (GMM_SM.getNumberOfComponents() == 0)
+ {
+ InitGMM(GMM_SM, GMM_N, 10);
+ }
+ static libRSF::SumMix1 NoisePseudorangeSumMix(GMM_SM);
+
+ Graph.addFactor(libRSF::FactorType::Pseudorange3, ListPseudorange, Pseudorange, NoisePseudorangeSumMix);
+
+ break;
+
+ case ROBUST_STSM:
+ static libRSF::GaussianMixture<1> GMM_SMST;
+ if (GMM_SMST.getNumberOfComponents() == 0)
+ {
+ InitGMM(GMM_SMST, GMM_N, 10);
+ }
+ static libRSF::SumMix1 NoisePseudorangeSumMixST(GMM_SMST);
+
+ Graph.addFactor(libRSF::FactorType::Pseudorange3, ListPseudorange, Pseudorange, NoisePseudorangeSumMixST);
+
+ break;
+
+ case ROBUST_STMM:
+ static libRSF::GaussianMixture<1> GMM_MMST;
+ if (GMM_MMST.getNumberOfComponents() == 0)
+ {
+ InitGMM(GMM_MMST, GMM_N, 10);
+ }
+ static libRSF::MaxMix1 NoisePseudorangeMaxMixST(GMM_MMST);
+
+ Graph.addFactor(libRSF::FactorType::Pseudorange3, ListPseudorange, Pseudorange, NoisePseudorangeMaxMixST);
+
+ break;
+
+ case ROBUST_STSM_VBI:
+ /** fill empty GMM */
+ static libRSF::GaussianMixture<1> GMM_SMST_VBI;
+ if (GMM_SMST_VBI.getNumberOfComponents() == 0)
+ {
+ InitGMM(GMM_SMST_VBI, 2, 10);
+ }
+ static libRSF::SumMix1 NoisePseudorangeSumMixST_VBI(GMM_SMST_VBI);
+
+ Graph.addFactor(libRSF::FactorType::Pseudorange3, ListPseudorange, Pseudorange, NoisePseudorangeSumMixST_VBI);
+
+ break;
+
+ case ROBUST_STMM_VBI:
+ /** fill empty GMM */
+ static libRSF::GaussianMixture<1> GMM_MMST_VBI;
+ if (GMM_MMST_VBI.getNumberOfComponents() == 0)
+ {
+ InitGMM(GMM_MMST_VBI, 2, 10);
+ }
+ static libRSF::MaxMix1 NoisePseudorangeMaxMixST_VBI(GMM_MMST_VBI);
+
+ Graph.addFactor(libRSF::FactorType::Pseudorange3, ListPseudorange, Pseudorange, NoisePseudorangeMaxMixST_VBI);
+
+ break;
+
+ default:
+ std::cerr << "Error: Wrong pseudorange error model model: " << Config.RangeErrorModel.Type << std::endl;
+ break;
+ }
+ }
+}
+
+/** @brief Use a GMM to estimate the error distribution of a factor
+*
+* @param Graph reference to the factor graph object
+* @param Config reference to the factor graph config object that specifies the motion model
+* @param Timestamp current timestamp
+* @return nothing
+*
+*/
+void TuneErrorModel(libRSF::FactorGraph &Graph,
+ libRSF::FactorGraphConfig &Config,
+ double Timestamp)
+{
+ if(Config.RangeErrorModel.Type == ROBUST_STSM || Config.RangeErrorModel.Type == ROBUST_STMM ||
+ Config.RangeErrorModel.Type == ROBUST_STSM_VBI || Config.RangeErrorModel.Type == ROBUST_STMM_VBI)
+ {
+ /** compute resudiuals of the factor graph */
+ std::vector ErrorData;
+ Graph.computeUnweightedError(libRSF::FactorType::Pseudorange3, ErrorData);
+
+ /** remove zeros in MM results (unused dimension!) */
+ if(Config.RangeErrorModel.Type == ROBUST_STMM || Config.RangeErrorModel.Type == ROBUST_STMM_VBI)
+ {
+ for(int i = ErrorData.size() - 1; i > 0; i -= 2)
+ {
+ ErrorData.erase(ErrorData.begin() + i);
+ }
+ }
+
+ if(Config.RangeErrorModel.Type == ROBUST_STSM || Config.RangeErrorModel.Type == ROBUST_STMM)
+ {
+ /** fill empty GMM */
+ libRSF::GaussianMixture<1> GMM;
+
+ if(GMM.getNumberOfComponents() == 0)
+ {
+ InitGMM(GMM, GMM_N, 10);
+ }
+
+ /** call the EM algorithm */
+ GMM.estimateWithEM(ErrorData);
+
+ /** remove offset of the first "LOS" component */
+ GMM.removeOffset();
+
+ /** apply error model */
+ if(Config.RangeErrorModel.Type == ROBUST_STSM)
+ {
+ libRSF::SumMix1 NewSMModel(GMM);
+ Graph.setNewErrorModel(libRSF::FactorType::Pseudorange3, NewSMModel);
+ }
+ else if(Config.RangeErrorModel.Type == ROBUST_STMM)
+ {
+ libRSF::MaxMix1 NewMMModel(GMM);
+ Graph.setNewErrorModel(libRSF::FactorType::Pseudorange3, NewMMModel);
+ }
+
+ }
+ else if(Config.RangeErrorModel.Type == ROBUST_STSM_VBI || Config.RangeErrorModel.Type == ROBUST_STMM_VBI)
+ {
+ /** initialize GMM */
+ static libRSF::GaussianMixture<1> GMMAdaptive;
+ libRSF::GaussianComponent<1> Component;
+
+ if(GMMAdaptive.getNumberOfComponents() == 0)
+ {
+ Component.setParamsStdDev((ceres::Vector(1) << 10.0).finished(),
+ (ceres::Vector(1) << 0.0).finished(),
+ (ceres::Vector(1) << 1.0).finished());
+ GMMAdaptive.addComponent(Component);
+ }
+
+ /** calculate statistics for GMM initialization*/
+ std::vector v = ErrorData;
+ double sum = std::accumulate(v.begin(), v.end(), 0.0);
+ double mean = sum / v.size();
+
+ std::vector diff(v.size());
+ std::transform(v.begin(), v.end(), diff.begin(), [mean](double x)
+ {
+ return x - mean;
+ });
+ double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
+ double stdev = std::sqrt(sq_sum / v.size());
+
+ /** add just one component per timestamp */
+ if(GMMAdaptive.getNumberOfComponents() >= VBI_N_MAX)
+ {
+ GMMAdaptive.sortComponentsByWeight();
+ GMMAdaptive.removeLastComponent();
+ }
+
+ Component.setParamsStdDev((ceres::Vector(1) << stdev).finished(),
+ (ceres::Vector(1) << 0.0).finished(),
+ (ceres::Vector(1) << 1.0/GMMAdaptive.getNumberOfComponents()).finished());
+
+
+ GMMAdaptive.addComponent(Component);
+
+ /** estimate GMM with Variational Inference */
+ GMMAdaptive.estimateWithVBI(ErrorData, VBI_NU);
+
+ /** remove offset*/
+ GMMAdaptive.removeOffset();
+
+ /** apply error model */
+ if(Config.RangeErrorModel.Type == ROBUST_STSM_VBI)
+ {
+ libRSF::SumMix1 NewSMModel(GMMAdaptive);
+ Graph.setNewErrorModel(libRSF::FactorType::Pseudorange3, NewSMModel);
+ }
+ else if(Config.RangeErrorModel.Type == ROBUST_STMM_VBI)
+ {
+ libRSF::MaxMix1 NewMMModel(GMMAdaptive);
+ Graph.setNewErrorModel(libRSF::FactorType::Pseudorange3, NewMMModel);
+ }
+ }
+ }
+}
+
+int main(int argc, char** argv)
+{
+ google::InitGoogleLogging(argv[0]);
+ libRSF::FactorGraphConfig Config;
+
+ /** process command line parameter */
+ if(!Config.ReadCommandLineOptions(argc, argv))
+ {
+ std::cout << std::endl;
+ return 1;
+ }
+
+ /** configure the solver */
+ ceres::Solver::Options SolverOptions;
+ SolverOptions.minimizer_progress_to_stdout = false;
+ SolverOptions.use_nonmonotonic_steps = true;
+ SolverOptions.trust_region_strategy_type = ceres::TrustRegionStrategyType::DOGLEG;
+ SolverOptions.dogleg_type = ceres::DoglegType::SUBSPACE_DOGLEG;
+ SolverOptions.num_threads = 8;
+ SolverOptions.max_num_iterations = 100;
+
+ /** read input data */
+ libRSF::SensorDataSet InputData;
+ libRSF::ReadDataFromFile(Config.InputFile, InputData);
+
+ /** Build optimization problem from sensor data */
+ ceres::Problem::Options Options;
+ Options.enable_fast_removal = true;
+ libRSF::FactorGraph Graph(Options);
+
+ libRSF::StateDataSet Result;
+ libRSF::SensorData DeltaTime;
+
+ double Timestamp, TimestampFirst, TimestampOld, TimestampLast;
+ InputData.getFirstTimestamp("Range 3D", TimestampFirst);
+ InputData.getLastTimestamp("Range 3D", TimestampLast);
+ Timestamp = TimestampFirst;
+ TimestampOld = TimestampFirst;
+ int nTimestamp = 0;
+
+ /** add fist variables and factors */
+ InitGraph(Graph, InputData, Config, SolverOptions, TimestampFirst);
+
+ /** solve multiple times with refined model to achieve good initial convergence */
+ Graph.solve(SolverOptions);
+ TuneErrorModel(Graph, Config, Timestamp);
+ Graph.solve(SolverOptions);
+
+ /** safe result at first timestamp */
+ Result.addElement(POSITION_STATE, Graph.getStateData().getElement(POSITION_STATE, Timestamp, 0));
+
+ /** get odometry noise from first measurement */
+ libRSF::SensorData Odom = InputData.getElement("Odometry 3D", Timestamp);
+ ceres::Vector StdOdom4DOF(4);
+ StdOdom4DOF << Odom.getStdDev().head(3), Odom.getStdDev().tail(1);
+ libRSF::GaussianDiagonal<4> NoiseOdom4DOF(StdOdom4DOF);
+
+ /** hard coded constant clock error drift (CCED) model noise properties */
+ ceres::Vector StdCCED(2);
+
+ if(strcmp(Config.InputFile, "Data_Chemnitz.txt") == 0)
+ {
+ StdCCED << 0.1, 0.009; /** set CCED standard deviation for Chemnitz City dataset */
+ }
+ else
+ {
+ StdCCED << 0.05, 0.01; /** set CCED standard deviation for smartLoc datasets */
+ }
+
+ libRSF::GaussianDiagonal<2> NoiseCCED(StdCCED);
+
+ /** iterate over timestamps */
+ while(InputData.getNextTimestamp("Range 3D", Timestamp, Timestamp))
+ {
+ DeltaTime = GenerateDeltaTime(TimestampOld, Timestamp);
+
+ /** add position, orientation and clock error */
+ Graph.addState(POSITION_STATE, libRSF::StateType::Pose3, Timestamp);
+ Graph.addState(CLOCK_ERROR_STATE, libRSF::StateType::Val1, Timestamp);
+ Graph.addState(ORIENTATION_STATE, libRSF::StateType::Angle, Timestamp);
+ Graph.addState(CLOCK_DRIFT_STATE, libRSF::StateType::Val1, Timestamp);
+
+ /** add odometry */
+ libRSF::StateList MotionList;
+ MotionList.add(POSITION_STATE, TimestampOld);
+ MotionList.add(POSITION_STATE, Timestamp);
+ MotionList.add(ORIENTATION_STATE, TimestampOld);
+ MotionList.add(ORIENTATION_STATE, Timestamp);
+ Graph.addFactor(libRSF::FactorType::Odom4, MotionList, InputData.getElement("Odometry 3D", Timestamp), DeltaTime, NoiseOdom4DOF);
+
+ /** add clock drift model */
+ libRSF::StateList ClockList;
+ ClockList.add(CLOCK_ERROR_STATE, TimestampOld);
+ ClockList.add(CLOCK_ERROR_STATE, Timestamp);
+ ClockList.add(CLOCK_DRIFT_STATE, TimestampOld);
+ ClockList.add(CLOCK_DRIFT_STATE, Timestamp);
+ Graph.addFactor(libRSF::FactorType::ConstDrift1, ClockList, DeltaTime, NoiseCCED);
+
+ /** add all pseudo range measurements of with current timestamp */
+ AddPseudorangeMeasurements(Graph, InputData, Config, Timestamp);
+
+ /** tune self-tuning error model */
+ TuneErrorModel(Graph, Config, Timestamp);
+
+ /** solve the estimation problem */
+ Graph.solve(SolverOptions);
+
+ /** save data after optimization */
+ Result.addElement(POSITION_STATE, Graph.getStateData().getElement(POSITION_STATE, Timestamp, 0));
+
+ /** apply sliding window */
+ Graph.removeAllFactorsOutsideWindow(60, Timestamp);
+ Graph.removeAllStatesOutsideWindow(60, Timestamp);
+
+ /** save time stamp */
+ TimestampOld = Timestamp;
+
+ nTimestamp++;
+ }
+
+ /** print last report */
+ Graph.printReport();
+
+ /** write results to disk */
+ libRSF::WriteDataToFile(Config.OutputFile, POSITION_STATE, Result);
+
+ return 0;
+}
diff --git a/applications/IV19_GNSS.h b/applications/IV19_GNSS.h
new file mode 100644
index 0000000..27ad0e1
--- /dev/null
+++ b/applications/IV19_GNSS.h
@@ -0,0 +1,84 @@
+/***************************************************************************
+ * libRSF - A Robust Sensor Fusion Library
+ *
+ * Copyright (C) 2018 Chair of Automation Technology / TU Chemnitz
+ * For more information see https://www.tu-chemnitz.de/etit/proaut/self-tuning
+ *
+ * libRSF is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * libRSF 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with libRSF. If not, see
+#include
+
+#include
+#include "../include/FactorGraph.h"
+#include "../include/FactorGraphConfig.h"
+#include "../include/FileAccess.h"
+#include "../include/StateDataSet.h"
+#include "../include/SensorDataSet.h"
+
+/** pre-defined state names */
+#define POSITION_STATE "Position"
+#define ORIENTATION_STATE "Orientation"
+#define CLOCK_ERROR_STATE "ClockError"
+#define CLOCK_DRIFT_STATE "ClockDrift"
+
+/** configuration */
+#define VBI_NU 2.0 /**< degrees of freedom of the Wishart prior */
+#define VBI_N_MAX 8 /**< maximum number of GMM components */
+
+#define GMM_N 3 /**< fixed number of components (which is used in static case) */
+
+/** Generates a delta time measurement object from two timestamps */
+libRSF::SensorData GenerateDeltaTime(const double TimestampOld,
+ const double TimestampNew);
+
+/** Adds a pseudorange measurement to the graph */
+void AddPseudorangeMeasurements(libRSF::FactorGraph& Graph,
+ libRSF::SensorDataSet & Measurements,
+ libRSF::FactorGraphConfig const &Config,
+ double Timestamp);
+
+/** use EM algorithm to tune the gaussian mixture model */
+void TuneErrorModel(libRSF::FactorGraph &Graph,
+ libRSF::FactorGraphConfig &Config,
+ double Timestamp);
+
+/** init zero mean GMM with increasing uncertainty in each component */
+template
+void InitGMM(libRSF::GaussianMixture &GMM, unsigned int NumberOfComponents, double BaseStdDev)
+{
+ libRSF::GaussianComponent Component;
+
+ /** construct Gaussian parameters */
+ Eigen::Matrix Mean = Eigen::Matrix::Zero();
+ Eigen::Matrix Weight;
+ Weight(0) = 1.0/NumberOfComponents;
+
+ Eigen::Matrix SqrtInfo = Eigen::Matrix::Identity() * (1.0/BaseStdDev);
+
+ GMM.clear();
+ for(int nComponent = 0; nComponent < NumberOfComponents; ++nComponent)
+ {
+ Component.setParamsSqrtInformation(SqrtInfo*std::pow(0.1, nComponent), Mean, Weight);
+ GMM.addComponent(Component);
+ }
+}
+
+#endif // ICRA19_GNSS_H_INCLUDED
diff --git a/datasets/Chemnitz City/readme.txt b/datasets/Chemnitz City/readme.txt
index 474e6a3..e746482 100644
--- a/datasets/Chemnitz City/readme.txt
+++ b/datasets/Chemnitz City/readme.txt
@@ -17,10 +17,10 @@ Every column holds an measurement element corresponding to its identifier:
1 - range3
2 - time stamp [s]
3 - pseudorange mean [m]
-3 - pseudorange standard deviation [m]
-4 - satellite position ECEF-X [m]
-5 - satellite position ECEF-Y [m]
-6 - satellite position ECEF-Z [m]
+4 - pseudorange standard deviation [m]
+5 - satellite position ECEF-X [m]
+6 - satellite position ECEF-Y [m]
+7 - satellite position ECEF-Z [m]
8 - satellite ID
9 - satellite elevation angle [deg]
diff --git a/datasets/Indoor UWB/readme.txt b/datasets/Indoor UWB/readme.txt
index 7067276..c112481 100644
--- a/datasets/Indoor UWB/readme.txt
+++ b/datasets/Indoor UWB/readme.txt
@@ -17,10 +17,10 @@ Every column holds an measurement element corresponding to its identifier:
1 - range2
2 - time stamp [s]
3 - pseudorange mean [m]
-3 - pseudorange standard deviation [m]
-4 - UWB module position X [m]
-5 - UWB module position Y [m]
-6 - UWB module ID
+4 - pseudorange standard deviation [m]
+5 - UWB module position X [m]
+6 - UWB module position Y [m]
+7 - UWB module ID
## odometry ##
1 - odom2diff
diff --git a/datasets/smartLoc/readme.txt b/datasets/smartLoc/readme.txt
index eb3b4cc..44a69a0 100644
--- a/datasets/smartLoc/readme.txt
+++ b/datasets/smartLoc/readme.txt
@@ -17,10 +17,10 @@ Every column holds an measurement element corresponding to its identifier:
1 - range3
2 - time stamp [s]
3 - pseudorange mean [m]
-3 - pseudorange standard deviation [m]
-4 - satellite position ECEF-X [m]
-5 - satellite position ECEF-Y [m]
-6 - satellite position ECEF-Z [m]
+4 - pseudorange standard deviation [m]
+5 - satellite position ECEF-X [m]
+6 - satellite position ECEF-Y [m]
+7 - satellite position ECEF-Z [m]
8 - satellite ID
9 - satellite elevation angle [deg]
10 - carrier-to-noise density ratio [dBHz]
diff --git a/include/FactorGraphConfig.h b/include/FactorGraphConfig.h
index 46d8e31..f76e4a6 100644
--- a/include/FactorGraphConfig.h
+++ b/include/FactorGraphConfig.h
@@ -43,12 +43,8 @@
#define ROBUST_SM 6
#define ROBUST_STSM 9
#define ROBUST_STMM 10
-#define RANGE_MM_GR 12
-#define RANGE_SM_GR 13
-#define RANGE_STMM_GR 14
-#define RANGE_STSM_GR 15
-#define RANGE_STSM_VBI 16
-#define RANGE_STMM_VBI 17
+#define ROBUST_STSM_VBI 16
+#define ROBUST_STMM_VBI 17
namespace libRSF
{
diff --git a/include/Geometry.h b/include/Geometry.h
index b8f7775..5710cd0 100644
--- a/include/Geometry.h
+++ b/include/Geometry.h
@@ -66,40 +66,6 @@ namespace libRSF
return RelativeMotion;
}
- template
- void VectorDifference(const T1* const Vector1, const T2* const Vector2, T1* Difference)
- {
- for(int nDim = 0; nDim < Dimension; nDim++)
- {
- Difference[nDim] = Vector1[nDim] - Vector2[nDim];
- }
- }
-
- template
- T VectorLength(const T* const Vector)
- {
- T SquaredSum = T(0.0);
-
- for(int nDim = 0; nDim < Dimension; nDim++)
- {
- SquaredSum += ceres::pow(Vector[nDim], 2);
- }
-
- /** for stability of the derivation */
- if(SquaredSum < T(1e-10))
- SquaredSum += T(1e-10);
-
- return ceres::sqrt(SquaredSum);
- }
-
- template
- T1 VectorDistance(const T1* const Vector1, const T2* const Vector2)
- {
- T1 Difference[Dimension];
- VectorDifference(Vector1, Vector2, Difference);
- return T1(VectorLength(Difference));
- }
-
}
#endif // GEOMETRY_H
diff --git a/include/QuaternionCalc.h b/include/QuaternionCalc.h
index 2df4f54..7133f09 100644
--- a/include/QuaternionCalc.h
+++ b/include/QuaternionCalc.h
@@ -34,26 +34,10 @@
#include
#include
#include "Geometry.h"
+#include "VectorMath.h"
namespace libRSF
{
-
- /** @brief Converts a quaterion to Tait–Bryan angles
- * From:
- * Wikipedia contributors,
- * "Conversion between quaternions and Euler angles"
- * Wikipedia, The Free Encyclopedia,
- * https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles&oldid=883423661
- * (accessed February 15, 2019).
- *
- * @param Quaternion ceres quaternion with ordering w,x,y,z
- * @param Yaw rotation around z
- * @param Pitch rotation around y
- * @param Roll rotation around x
- */
- template
- inline void QuaternionToRPY(const T* Quaternion, T* Roll, T* Pitch, T* Yaw);
-
/** @brief invert a quaternion
*
* @param Quaternion ceres quaternion with ordering w,x,y,z
@@ -65,36 +49,6 @@ namespace libRSF
template
inline T RotationBetween(const T* Vector1, const T* Vector2, T* Quaternion);
- template
- inline void QuaternionToRPY(const T* Quaternion, T* Roll, T* Pitch, T* Yaw)
- {
-
- const T& Qw = Quaternion[0];
- const T& Qx = Quaternion[1];
- const T& Qy = Quaternion[2];
- const T& Qz = Quaternion[3];
-
- /** conversion from Wikipedia */
- // roll (x-axis rotation)
- T sinr_cosp = +2.0 * (Qw * Qx + Qy * Qz);
- T cosr_cosp = +1.0 - 2.0 * (Qx * Qx + Qy * Qy);
- Roll[0] = ceres::atan2(sinr_cosp, cosr_cosp);
-
- // pitch (y-axis rotation)
- T sinp = +2.0 * (Qw * Qy - Qz * Qx);
- if (sinp >= T(1))
- Pitch[0] = T(M_PI / 2); // use 90 degrees if out of range
- else if (sinp <= T(-1))
- Pitch[0] = T(-M_PI / 2);
- else
- Pitch[0] = ceres::asin(sinp);
-
- // yaw (z-axis rotation)
- T siny_cosp = +2.0 * (Qw * Qz + Qx * Qy);
- T cosy_cosp = +1.0 - 2.0 * (Qy * Qy + Qz * Qz);
- Yaw[0] = ceres::atan2(siny_cosp, cosy_cosp);
- }
-
template
inline void InvertQuaterion(const T* Quaternion, T* QuaternionInv)
{
@@ -116,7 +70,7 @@ namespace libRSF
template
inline T NormalizeQuaternion(T* Quaternion)
{
- const T Norm = VectorDistance<4, T, T>(Quaternion, Quaternion);
+ const T Norm = VectorLength<4, T>(Quaternion);
Quaternion[0] /= Norm;
Quaternion[1] /= Norm;
Quaternion[2] /= Norm;
diff --git a/include/VectorMath.h b/include/VectorMath.h
index 4ebc36d..b594bd1 100644
--- a/include/VectorMath.h
+++ b/include/VectorMath.h
@@ -1,56 +1,98 @@
/***************************************************************************
- * libRSF - A Robust Sensor Fusion Libary
- *
- * Copyright (C) 2018 Chair of Automation Technology / TU Chemnitz
+ * libRSF - A Robust Sensor Fusion Library
+ *
+ * Copyright (C) 2019 Chair of Automation Technology / TU Chemnitz
* For more information see https://www.tu-chemnitz.de/etit/proaut/self-tuning
- *
+ *
* libRSF is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
- *
+ *
* libRSF 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 General Public License for more details.
- *
+ *
* You should have received a copy of the GNU General Public License
* along with libRSF. If not, see
+#include
#include
-namespace libisf2
+
+namespace libRSF
{
+ /** define vector and matrix types for libRSF */
+ typedef Eigen::Matrix Vector;
+ typedef Eigen::Matrix Vector1;
+ typedef Eigen::Matrix Vector2;
+ typedef Eigen::Matrix Vector3;
+ typedef Eigen::Matrix Vector4;
+ typedef Eigen::Matrix Vector6;
+ typedef Eigen::Matrix Vector9;
+ typedef Eigen::Matrix Vector12;
+ typedef Eigen::Matrix Vector15;
+
+ typedef Eigen::Matrix Matrix;
+ typedef Eigen::Matrix Matrix11;
+ typedef Eigen::Matrix Matrix22;
+ typedef Eigen::Matrix Matrix33;
+ typedef Eigen::Matrix Matrix44;
+
+ /** math with vector/matrix class */
+ template
+ Eigen::Matrix SquareRoot(Eigen::Matrix A)
+ {
+ Eigen::SelfAdjointEigenSolver> ES(A);
+ return ES.operatorSqrt();
+ }
+
+ template
+ Eigen::Matrix InverseSquareRoot(Eigen::Matrix A)
+ {
+ Eigen::SelfAdjointEigenSolver> ES(A);
+ return ES.operatorInverseSqrt();
+ }
+
+ /** vector math with raw pointers (compatible to ceres jet type) */
template
void VectorDifference(const T1* const Vector1, const T2* const Vector2, T1* Difference)
{
- for(int nDim = 0; nDim < Dimension; nDim++)
- {
- Difference[nDim] = Vector1[nDim] - Vector2[nDim];
- }
+ Eigen::Map > V1(Vector1);
+ Eigen::Map > V2(Vector2);
+ Eigen::Map > Diff(Difference);
+
+ Diff = V1 - V2;
}
template
T VectorLength(const T* const Vector)
{
- T SquaredSum = T(0.0);
-
- for(int nDim = 0; nDim < Dimension; nDim++)
- {
- SquaredSum += ceres::pow(Vector[nDim], 2);
- }
+ Eigen::Map > V(Vector);
+ T SquaredNorm = V.squaredNorm();
/** for stability of the derivation */
- if(SquaredSum < T(1e-10))
- SquaredSum += T(1e-10);
+ if(SquaredNorm < T(1e-10))
+ SquaredNorm += 1e-10;
- return ceres::sqrt(SquaredSum);
+ return ceres::sqrt(SquaredNorm);
}
template
@@ -58,8 +100,10 @@ namespace libisf2
{
T1 Difference[Dimension];
VectorDifference(Vector1, Vector2, Difference);
- return T1(VectorLength(Difference));
+ return VectorLength(Difference);
}
+
+ void RemoveColumn (Eigen::MatrixXd& Matrix, unsigned int ColToRemove);
}
#endif // VECTORMATH_H
diff --git a/include/error_models/GaussianComponent.h b/include/error_models/GaussianComponent.h
index 428a87d..f0050e5 100644
--- a/include/error_models/GaussianComponent.h
+++ b/include/error_models/GaussianComponent.h
@@ -34,6 +34,7 @@
#include
#include
+#include "../VectorMath.h"
namespace libRSF
{
@@ -48,7 +49,7 @@ namespace libRSF
~GaussianComponent() {};
- void setParamsStdDev(ceres::Vector StdDev, ceres::Vector Mean, ceres::Vector Weight)
+ void setParamsStdDev(Vector StdDev, Vector Mean, Vector Weight)
{
_Mean = Mean;
_Weight = Weight;
@@ -59,6 +60,24 @@ namespace libRSF
updateScaling();
}
+ void setParamsInformation(ceres::Matrix Information, Vector Mean, Vector Weight)
+ {
+ _Mean = Mean;
+ _Weight = Weight;
+ _SqrtInformation = SquareRoot(Information);
+
+ updateScaling();
+ }
+
+ void setParamsSqrtInformation(ceres::Matrix SqrtInformation, Vector Mean, Vector Weight)
+ {
+ _Mean = Mean;
+ _Weight = Weight;
+ _SqrtInformation = SqrtInformation;
+
+ updateScaling();
+ }
+
Eigen::Matrix getMean() const
{
return _Mean;
@@ -83,11 +102,12 @@ namespace libRSF
template
Eigen::Matrix < T, Dimension, 1 > getExponentialPart(T * const Error) const
{
- Eigen::Map > ErrorMap(Error);
+ Eigen::Map > ErrorMap(Error);
Eigen::Matrix WeightedError;
/** shift by mean */
WeightedError = ErrorMap + _Mean.template cast();
+
/** scale with information matrix */
WeightedError = _SqrtInformation.template cast() * WeightedError;
@@ -142,7 +162,7 @@ namespace libRSF
Covariance += (Errors.row(n) + _Mean).transpose() * (Errors.row(n) + _Mean) * Likelihoods(n);
}
Covariance.array() /= LikelihoodSum;
- _SqrtInformation = Covariance.inverse().llt().matrixL();
+ _SqrtInformation = InverseSquareRoot(Covariance);
/** check for degenerated SqrtInfo matrix */
if(!(_SqrtInformation.array().isFinite().all()))
diff --git a/include/error_models/GaussianMixture.h b/include/error_models/GaussianMixture.h
index 64655d6..adbbe10 100644
--- a/include/error_models/GaussianMixture.h
+++ b/include/error_models/GaussianMixture.h
@@ -35,6 +35,8 @@
#include
#include
+#include
+
#include "GaussianComponent.h"
#include "../StateData.h"
@@ -57,6 +59,11 @@ namespace libRSF
_Mixture.emplace_back(Gaussian);
}
+ void removeLastComponent()
+ {
+ _Mixture.pop_back();
+ }
+
void clear()
{
_Mixture.clear();
@@ -139,7 +146,6 @@ namespace libRSF
double LikelihoodChange = std::abs(LikelihoodSumOld - LikelihoodSumNew)/LikelihoodSumNew;
- /** remove small components */
if(Adaptive)
{
for(size_t m = 0; m < M; ++m)
@@ -175,6 +181,18 @@ namespace libRSF
return false;
}
+ /** variational bayesian inference */
+ void estimateWithVBI(std::vector &Data, double Nu);
+
+ void printParameter()
+ {
+ std::cout << "Mean" <<' ' << "StdDev" <<' ' << "Weight" <
Eigen::Matrix < T, Dimension, 1 > getExponentialPartOfComponent(size_t NumberOfComponent, T * const Error) const
diff --git a/include/factors/ConstantDriftFactor.h b/include/factors/ConstantDriftFactor.h
index 42490d9..8c16e27 100644
--- a/include/factors/ConstantDriftFactor.h
+++ b/include/factors/ConstantDriftFactor.h
@@ -34,7 +34,7 @@
#include
#include "MeasurementFactor.h"
-#include "../Geometry.h"
+#include "../VectorMath.h"
namespace libRSF
{
diff --git a/include/factors/OdometryFactor3D.h b/include/factors/OdometryFactor3D.h
index 44a620d..c7e6624 100644
--- a/include/factors/OdometryFactor3D.h
+++ b/include/factors/OdometryFactor3D.h
@@ -67,9 +67,9 @@ namespace libRSF
{
T PositionError[3];
T PositionErrorBody[3];
- T QuartOldInv [4];
- T QuartPose [4];
- T ZAxis[3], QuartOld[4], QuartYaw[4];
+ T QuatOldInv [4];
+ T QuatPose [4];
+ T ZAxis[3], QuatOld[4], QuatYaw[4];
Eigen::Matrix ErrorVector, Displacement;
@@ -78,17 +78,17 @@ namespace libRSF
ZAxis[1] = T(0.0);
ZAxis[2] = T(1.0);
- RotationBetween(ZAxis, Pos1, QuartPose);
+ RotationBetween(ZAxis, Pos1, QuatPose);
- QuartYaw[0] = ceres::cos(Yaw1[0] / 2.0);
- QuartYaw[1] = T(0.0);
- QuartYaw[2] = T(0.0);
- QuartYaw[3] = ceres::sin(Yaw1[0] / 2.0);
+ QuatYaw[0] = ceres::cos(Yaw1[0] / 2.0);
+ QuatYaw[1] = T(0.0);
+ QuatYaw[2] = T(0.0);
+ QuatYaw[3] = ceres::sin(Yaw1[0] / 2.0);
- ceres::QuaternionProduct(QuartPose, QuartYaw, QuartOld);
+ ceres::QuaternionProduct(QuatPose, QuatYaw, QuatOld);
/** invert quaternion */
- libRSF::InvertQuaterion(QuartOld, QuartOldInv);
+ libRSF::InvertQuaterion(QuatOld, QuatOldInv);
/** calculate translation */
PositionError[0] = Pos2[0] - Pos1[0];
@@ -96,7 +96,7 @@ namespace libRSF
PositionError[2] = Pos2[2] - Pos1[2];
/** rotate translation back in body frame */
- ceres::QuaternionRotatePoint(QuartOldInv, PositionError, PositionErrorBody);
+ ceres::QuaternionRotatePoint(QuatOldInv, PositionError, PositionErrorBody);
/** weight position error */
Displacement[0] = PositionErrorBody[0] ;
diff --git a/include/factors/PseudorangeFactor.h b/include/factors/PseudorangeFactor.h
index 9736587..bf1526c 100644
--- a/include/factors/PseudorangeFactor.h
+++ b/include/factors/PseudorangeFactor.h
@@ -34,7 +34,7 @@
#include
#include "MeasurementFactor.h"
-#include "../Geometry.h"
+#include "../VectorMath.h"
#include "RangeFactor.h"
/** physical constants for sagnac effect */
diff --git a/include/factors/RangeFactor.h b/include/factors/RangeFactor.h
index adf4812..692b9b5 100644
--- a/include/factors/RangeFactor.h
+++ b/include/factors/RangeFactor.h
@@ -34,7 +34,7 @@
#include
#include "MeasurementFactor.h"
-#include "../Geometry.h"
+#include "../VectorMath.h"
namespace libRSF
{
diff --git a/include/predictors/PredictOdometry.h b/include/predictors/PredictOdometry.h
index 06ab4d3..d76e56d 100644
--- a/include/predictors/PredictOdometry.h
+++ b/include/predictors/PredictOdometry.h
@@ -34,6 +34,7 @@
#include
#include "../Geometry.h"
+#include "../VectorMath.h"
namespace libRSF
{
@@ -44,18 +45,18 @@ namespace libRSF
void PredictOdometry4DOFGlobal(const T* PoseOld, T* PoseNew, const T* YawOld, T* YawNew, const ceres::Vector &Odometry, double DeltaTime)
{
Eigen::Matrix ZAxis, Axis;
- Eigen::Matrix QuartPose, QuartYaw, QuartOld;
+ Eigen::Matrix QuatPose, QuatYaw, QuatOld;
- QuartYaw << cos(YawOld[0]/2.0), T(0), T(0), sin(YawOld[0]/2.0);
+ QuatYaw << cos(YawOld[0]/2.0), T(0), T(0), sin(YawOld[0]/2.0);
ZAxis << T(0), T(0), T(1);
ceres::CrossProduct(ZAxis.data(), PoseOld, Axis.data());
- QuartPose[0] = ceres::DotProduct(ZAxis.data(), PoseOld) + VectorLength<3>(ZAxis.data())*VectorLength<3>(PoseOld);
- QuartPose[1] = Axis[0];
- QuartPose[2] = Axis[1];
- QuartPose[3] = Axis[2];
- ceres::QuaternionProduct(QuartPose.data(), QuartYaw.data(), QuartOld.data());
+ QuatPose[0] = ceres::DotProduct(ZAxis.data(), PoseOld) + VectorLength<3>(ZAxis.data())*VectorLength<3>(PoseOld);
+ QuatPose[1] = Axis[0];
+ QuatPose[2] = Axis[1];
+ QuatPose[3] = Axis[2];
+ ceres::QuaternionProduct(QuatPose.data(), QuatYaw.data(), QuatOld.data());
T Translation[3], TranslationRot[3];
@@ -63,7 +64,7 @@ namespace libRSF
Translation[1] = T(Odometry[1]*DeltaTime);
Translation[2] = T(Odometry[2]*DeltaTime);
- ceres::QuaternionRotatePoint(QuartOld.data(), Translation, TranslationRot);
+ ceres::QuaternionRotatePoint(QuatOld.data(), Translation, TranslationRot);
PoseNew[0] = TranslationRot[0] + PoseOld[0];
PoseNew[1] = TranslationRot[1] + PoseOld[1];
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 78850a1..a88e707 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -35,6 +35,7 @@ set(SOURCEFILES
FileAccess.cpp
Geometry.cpp
ListInTime.cpp
+ VectorMath.cpp
)
# factors of the graph
diff --git a/src/FactorGraphConfig.cpp b/src/FactorGraphConfig.cpp
index 2ce18c9..3d8ded8 100644
--- a/src/FactorGraphConfig.cpp
+++ b/src/FactorGraphConfig.cpp
@@ -75,6 +75,14 @@ namespace libRSF
{
RangeErrorModel.Type = ROBUST_STMM;
}
+ else if(strcmp(argv[ArgCounter], "stmm_vbi") == 0)
+ {
+ RangeErrorModel.Type = ROBUST_STMM_VBI;
+ }
+ else if(strcmp(argv[ArgCounter], "stsm_vbi") == 0)
+ {
+ RangeErrorModel.Type = ROBUST_STSM_VBI;
+ }
else
{
std::cerr << "Wrong range error model: " << argv[ArgCounter];
diff --git a/src/VectorMath.cpp b/src/VectorMath.cpp
index 2cae39d..70b7acf 100644
--- a/src/VectorMath.cpp
+++ b/src/VectorMath.cpp
@@ -1,23 +1,37 @@
/***************************************************************************
- * libRSF - A Robust Sensor Fusion Libary
- *
- * Copyright (C) 2018 Chair of Automation Technology / TU Chemnitz
+ * libRSF - A Robust Sensor Fusion Library
+ *
+ * Copyright (C) 2019 Chair of Automation Technology / TU Chemnitz
* For more information see https://www.tu-chemnitz.de/etit/proaut/self-tuning
- *
+ *
* libRSF is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
- *
+ *
* libRSF 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 General Public License for more details.
- *
+ *
* You should have received a copy of the GNU General Public License
* along with libRSF. If not, see &Data, double Nu)
+ {
+ /** only 1D problems !!! */
+ #define Dimension 1
+
+ /** set up problem */
+ size_t N = Data.size();
+ size_t M = _Mixture.size();
+
+ /** N x M matrix */
+ Eigen::MatrixXd Likelihood;
+ Likelihood.resize(N, M);
+
+ /** map data to eigen vector */
+ ErrorMatType DataVector = Eigen::Map(Data.data(), N, Dimension);
+ /** change sign to match own GMM definition */
+ DataVector.array() *= -1;
+
+ double DataMean = DataVector.array().mean();
+ double DataCov = (DataVector.array() - DataMean).square().sum()/(DataVector.size()-1);
+
+ /** convergence criterion */
+ double const LikelihoodTolerance = 1e-6;
+ int const MaxIterations = 1000;
+
+ /** pruning criterion */
+ double MinWeight = 1.0/N;
+
+ /** define priors */
+ double const Beta = 1e-6; /**< prior over mean */
+ double const V = DataCov/Nu; /**< Wishart prior for I */
+
+ /** initialize variables */
+ std::vector NuInfo;
+ std::vector VInfo;
+ std::vector InfoMean;
+ std::vector MeanMean;
+ std::vector Weights;
+
+ /** initialize estimated parameters */
+ for(size_t m = 0; m < M; ++m)
+ {
+ NuInfo.push_back(Nu);
+ VInfo.push_back(V);
+ InfoMean.push_back(0.0); /**< will be overwritten */
+ MeanMean.push_back(0.0); /**< will be overwritten */
+ Weights.push_back(_Mixture.at(m).getWeight()(0));
+ }
+
+ /** convergence criteria (not variational!)*/
+ double LikelihoodSumOld = 1e40;
+ double LikelihoodSumNew;
+
+ /** first likelihood estimation is not variational! */
+ LikelihoodSumNew = computeLikelihood(DataVector, Likelihood);
+
+ /** repeat until convergence or 100 iterations*/
+ int i;
+ for(i = 0; i < MaxIterations; ++i)
+ {
+ bool ReachedConvergence = false;
+ bool PerfomedRemove = false;
+
+ /** Expectation step */
+
+ /** pre-calculate some multi-use variables */
+ Eigen::VectorXd SumLike = Likelihood.colwise().sum();
+ Eigen::VectorXd SumLikeX = (Likelihood.array().colwise() * DataVector.array()).colwise().sum();
+ Eigen::VectorXd SumLikeXX = (Likelihood.array().colwise() * DataVector.array().square()).colwise().sum();
+
+ double MuMuEx, InfoEx, LogInfoEx;
+ /** iterate over GMM components */
+ for(size_t m = 0; m < M; ++m)
+ {
+
+ /** calculate mean */
+ InfoMean.at(m) = Beta + NuInfo.at(m) / VInfo.at(m) * SumLike(m);
+ MeanMean.at(m) = NuInfo.at(m) / VInfo.at(m) / InfoMean.at(m) * SumLikeX(m);
+
+ /** calculate variance */
+ NuInfo.at(m) = Nu + SumLike(m);
+ MuMuEx = 1.0/InfoMean.at(m) + MeanMean.at(m)*MeanMean.at(m);
+
+ VInfo.at(m) = V + SumLikeXX(m) - 2*MeanMean.at(m)*SumLikeX(m) + MuMuEx * SumLike(m);
+
+ /** variational likelihood */
+ InfoEx = NuInfo.at(m)/VInfo.at(m);
+ LogInfoEx = Eigen::numext::digamma(NuInfo.at(m)/2.0) + log(2.0) - log(VInfo.at(m));
+
+ Likelihood.col(m) = (LogInfoEx/2.0
+ + log(Weights.at(m))
+ - 0.5*InfoEx*
+ (
+ DataVector.array().square()
+ - 2.0 * MeanMean.at(m) * DataVector.array()
+ + MuMuEx
+ )
+ ).array().exp();
+ }
+
+ /** "Maximization step" */
+
+ /** remove NaNs */
+ Likelihood = (Likelihood.array().isFinite()).select(Likelihood, 0.0);
+
+ /** calculate relative likelihood */
+ LikelihoodSumNew = Likelihood.sum();
+ Eigen::VectorXd LikelihoodRowSum = Likelihood.rowwise().sum();
+
+ for(size_t m = 0; m < M; ++m)
+ {
+ Likelihood.col(m).array() /= LikelihoodRowSum.array();
+ }
+
+ /** remove NaNs (occur if sum of likelihoods is zero) */
+ Likelihood = (Likelihood.array().isFinite()).select(Likelihood, 1.0/M);
+
+ /** calculate weights */
+ for(size_t m = 0; m < M; ++m)
+ {
+ Weights.at(m)= Likelihood.col(m).sum() / Likelihood.rows();
+ }
+
+ /** remove useless components */
+ for(int m = M-1; m >=0 ; --m)
+ {
+ if(Weights.at(m) < MinWeight)
+ {
+ NuInfo.erase(NuInfo.begin() + m);
+ VInfo.erase(VInfo.begin() + m);
+ InfoMean.erase(InfoMean.begin() + m);
+ MeanMean.erase(MeanMean.begin() + m);
+ Weights.erase(Weights.begin() + m);
+ RemoveColumn(Likelihood,m);
+ _Mixture.erase(_Mixture.begin() + m);
+
+ M -= 1;
+ /** enforce an additional iteration after removal and reset likelihood */
+ PerfomedRemove = true;
+ LikelihoodSumNew = 1e40;
+ }
+ }
+
+ /** check for convergence */
+ double LikelihoodChange = std::abs(LikelihoodSumOld - LikelihoodSumNew)/LikelihoodSumNew;
+
+ if(LikelihoodChange < LikelihoodTolerance)
+ {
+ ReachedConvergence = true;
+ }
+ else
+ {
+ ReachedConvergence = false;
+ }
+
+ /** terminate loop */
+ if(ReachedConvergence && !PerfomedRemove)
+ {
+ break;
+ }
+ else
+ {
+ LikelihoodSumOld = LikelihoodSumNew;
+ }
+
+ }
+
+ /** update mixtures */
+ for(size_t m = 0; m < M; ++m)
+ {
+ Eigen::Matrix Mean;
+ Eigen::Matrix Info;
+ Eigen::Matrix Weight;
+
+ Mean << MeanMean.at(m);
+ Info << NuInfo.at(m)/VInfo.at(m);
+ Weight << Weights.at(m);
+
+ _Mixture.at(m).setParamsInformation(Info, Mean, Weight);
+ }
+
+ }
}