Fix/imu ros test

dependencies.rosinstall

@@ -2,8 +2,8 @@
     local-name: catkin_simple
     uri: https://github.com/catkin/catkin_simple.git
 - git:
-    local-name: Schweizer-Messer
-    uri: https://github.com/ethz-asl/Schweizer-Messer.git
+    local-name: schweizer_messer
+    uri: https://github.com/ethz-asl/schweizer_messer.git
 - git:
     local-name: eigen_catkin
     uri: https://github.com/ethz-asl/eigen_catkin.git

oomact/CMakeLists.txt

@@ -21,21 +21,13 @@ endif()
 
 cs_add_library(${PROJECT_NAME}
   src/algo/OdometryPath.cpp
-  src/algo/OdometryPath.cpp
-  src/algo/PredictionWriter.cpp
   src/algo/PredictionWriter.cpp
   src/algo/splinesToFile.cpp
-  src/algo/splinesToFile.cpp
   src/CalibrationConfI.cpp
   src/calibrator/AbstractCalibrator.cpp
-  src/calibrator/AbstractCalibrator.cpp
   src/calibrator/BatchCalibrator.cpp
-  src/calibrator/BatchCalibrator.cpp
-  src/data/MapStorage.cpp
   src/data/MapStorage.cpp
   src/data/ObservationManagerI.cpp
-  src/data/ObservationManagerI.cpp
-  src/data/StorageI.cpp
   src/data/StorageI.cpp
   src/error-terms/ErrorTermAccelerometer.cpp
   src/error-terms/ErrorTermAngularVelocity.cpp
@@ -82,6 +74,7 @@ cs_add_library(${PROJECT_NAME}
   src/test/TestData.cpp
   src/test/Tools.cpp
   src/tools/CheckNotNull.cpp
+  src/tools/Covariance.cpp
   src/tools/ErrorTermStatistics.cpp
   src/tools/ErrorTermStatisticsWithProblemAndPredictor.cpp
   src/tools/Interval.cpp

oomact/include/aslam/calibration/algo/PredictionWriter.h

@@ -9,8 +9,8 @@
 
 #include <boost/shared_ptr.hpp>
 
-#include "aslam/calibration/error-terms/ConditionalErrorTerm.hpp"
-#include "aslam/calibration/Timestamp.hpp"
+#include "aslam/calibration/error-terms/ConditionalErrorTerm.h"
+#include "aslam/calibration/Timestamp.h"
 
 namespace aslam {
 namespace backend {

oomact/include/aslam/calibration/calibrator/AbstractCalibrator.h

@@ -1,9 +1,9 @@
 #ifndef H2E153EC9_9902_41A1_A522_E4E0A04D6F76
 #define H2E153EC9_9902_41A1_A522_E4E0A04D6F76
 
-#include "CalibratorI.hpp"
+#include "CalibratorI.h"
 #include "../algo/PredictionWriter.h"
-#include "../SensorId.hpp"
+#include "../SensorId.h"
 
 namespace aslam {
 namespace backend {
@@ -42,11 +42,19 @@ class AbstractCalibratorOptions : public CalibratorOptionsI {
   void setAcceptConstantErrorTerms(bool acceptConstantErrorTerms) {
     this->acceptConstantErrorTerms = acceptConstantErrorTerms;
   }
+
+  int getNumThreads() const override {
+    return numThreads;
+  }
+  void setNumThreads(int numThreads) {
+    this->numThreads = numThreads;
+  }
  private:
   bool predictResults;
   bool verbose;
   bool acceptConstantErrorTerms;
   double splineOutputSamplePeriod;
+  int numThreads;
 };
 
 class AbstractCalibrator : public virtual CalibratorI {
@@ -67,6 +75,8 @@ class AbstractCalibrator : public virtual CalibratorI {
   const Model & getModel() const override { return _model; }
   Model & getModel() override { return _model; }
 
+  virtual ValueStoreRef getValueStore() const override;
+
   double secsSinceStart(Timestamp timestamp) const override;
   std::string secsSinceStart(const Interval & interval) const override;
 
@@ -99,6 +109,8 @@ class AbstractCalibrator : public virtual CalibratorI {
 
   const std::shared_ptr<Model> _modelSP;
   Model & _model;
+
+  ValueStoreRef _config;
  private:
   void addMeasurementTimestamp(Timestamp lowerBound, Timestamp upperBound = InvalidTimestamp());
 

oomact/include/aslam/calibration/CalibrationConfI.h → oomact/include/aslam/calibration/calibrator/CalibrationConfI.h

@@ -1,7 +1,7 @@
 #ifndef H2FC5F3AD_D310_4A93_B2E5_55925AA5F4DF
 #define H2FC5F3AD_D310_4A93_B2E5_55925AA5F4DF
 #include <aslam/calibration/model/Module.h>
-#include "tools/Printable.h"
+#include "../tools/Printable.h"
 
 namespace aslam {
 namespace calibration {
@@ -33,10 +33,14 @@ class CalibrationConfI : public virtual Printable {
 
   virtual std::string getOutputFolder(size_t segmentIndex = 0) const = 0;
 
+  virtual bool shouldAnySensorBeRegisteredTo(const Sensor & to) const = 0;
   virtual bool shouldSensorsBeRegistered(const Sensor & from, const Sensor & to) const = 0;
 
   virtual bool getUseCalibPriors() const = 0;
 
+  virtual CalibratorI & getCalibrator() = 0;
+  virtual const CalibratorI & getCalibrator() const = 0;
+
   virtual ~CalibrationConfI();
 };
 

oomact/include/aslam/calibration/calibrator/CalibratorI.hpp → oomact/include/aslam/calibration/calibrator/CalibratorI.h

@@ -8,8 +8,8 @@
 
 #include "../data/ObservationManagerI.h"
 #include "../model/Model.h"
-#include "../SensorId.hpp"
-#include "../tools/Interval.hpp"
+#include "../SensorId.h"
+#include "../tools/Interval.h"
 
 namespace sm {
   class MatrixArchive;
@@ -21,8 +21,6 @@ using sm::value_store::ValueStoreRef;
 
 class Model;
 class CalibrationVariable;
-class CalibratorOptions;
-class MotionCaptureSource;
 class ModuleList;
 class PredictionFunctorWriter;
 
@@ -37,6 +35,7 @@ class CalibratorOptionsI {
   virtual bool getVerbose() const = 0;
   virtual void setVerbose(bool verbose) = 0;
   virtual bool getAcceptConstantErrorTerms() const = 0;
+  virtual int getNumThreads() const = 0;
 };
 
 class CalibratorI : public ObservationManagerI {
@@ -46,6 +45,8 @@ class CalibratorI : public ObservationManagerI {
   virtual Model & getModel() = 0;
   virtual const Model & getModel() const = 0;
 
+  virtual ValueStoreRef getValueStore() const = 0; // TODO D this should be a ConstValueStore once it exists
+
   virtual void setUpdateHandler(StatusUpdateHandler statusUpdateHandler, CalibrationUpdateHandler calibrationUpdateHandler) = 0;
 
   virtual void addToArchive(sm::MatrixArchive & ma, bool append = false) const = 0;

oomact/include/aslam/calibration/calibrator/CalibratorRef.h

@@ -0,0 +1,25 @@
+#ifndef HAA39AA34_DD3B_4A27_95E3_90FD35D78BC4
+#define HAA39AA34_DD3B_4A27_95E3_90FD35D78BC4
+
+namespace aslam {
+namespace calibration {
+class CalibratorI;
+
+class CalibratorRef {
+ public:
+  CalibratorRef(CalibratorI& calib) : calib_(calib) {}
+
+  CalibratorI & getCalibrator() {
+    return calib_;
+  }
+  const CalibratorI & getCalibrator() const {
+    return calib_;
+  }
+ private:
+  CalibratorI& calib_;
+};
+
+}
+}
+
+#endif /* HAA39AA34_DD3B_4A27_95E3_90FD35D78BC4 */

oomact/include/aslam/calibration/model/SetActivator.h → oomact/include/aslam/calibration/calibrator/SetActivator.h

@@ -4,7 +4,7 @@
 #include <iosfwd>
 #include <unordered_set>
 
-#include <aslam/calibration/CalibrationConfI.h>
+#include <aslam/calibration/calibrator/CalibrationConfI.h>
 
 namespace aslam {
 namespace calibration {

oomact/include/aslam/calibration/model/StateCarrier.h → oomact/include/aslam/calibration/calibrator/StateCarrier.h

@@ -5,7 +5,7 @@
 #include <memory>
 #include <string>
 
-#include <aslam/calibration/model/Sensor.hpp>
+#include <aslam/calibration/model/Sensor.h>
 
 namespace aslam {
 namespace calibration {

oomact/include/aslam/calibration/data/MapStorage.h

@@ -27,34 +27,35 @@ class MapStorageUnsafe {
 
 /**
  * The MapStorage class provides a simple implementation of the StorageI interface based on a std::unordered_map.
- * The template layer only requires the Key type. The actual work is done by MapStorageBase.
+ * The template layer only requires the Key type. The actual work is done by internal::MapStorageUnsafe.
  */
 template <typename Key>
 class MapStorage : public StorageI<Key> {
  public:
   virtual ~MapStorage() = default;
 
-  virtual void remove(Key key) override {
+  void remove(Key key) override {
     impl.remove(convertKey(key));
   }
   void clear() {
     impl.clear();
   }
- private:
-  static size_t convertKey(Key key) {
-    return reinterpret_cast<size_t>(key);
-  }
 
   void * get(Key key) const override {
     return impl.get(convertKey(key));
   }
+
   void add(Key key, StorageElement && data) override {
     impl.add(convertKey(key), std::move(data));
   }
 
-  virtual size_t size() const override {
+  size_t size() const override {
     return impl.size();
   }
+ private:
+  static size_t convertKey(Key key) {
+    return reinterpret_cast<size_t>(key);
+  }
 
   internal::MapStorageUnsafe impl;
 };

oomact/include/aslam/calibration/data/MeasurementsContainer.h

@@ -5,7 +5,7 @@
 #include <utility>
 #include <vector>
 
-#include "../Timestamp.hpp"
+#include "../Timestamp.h"
 
 namespace aslam {
 namespace calibration {

oomact/include/aslam/calibration/data/ObservationManagerI.h

@@ -4,7 +4,7 @@
 #include <string>
 #include <aslam/calibration/model/Module.h>
 
-#include "../Timestamp.hpp"
+#include "../Timestamp.h"
 
 namespace aslam {
 namespace calibration {
@@ -18,10 +18,15 @@ class ObservationManagerI {
   virtual ~ObservationManagerI();
 
   virtual ModuleStorage & getCurrentStorage() = 0;
+  virtual const ModuleStorage & getCurrentStorage() const = 0;
 
   virtual void setLowestTimestamp(Timestamp lowestTimeStamp) = 0;
   virtual void addMeasurementTimestamp(Timestamp t, const Sensor & sensor) = 0;
 
+  virtual bool isNextWindowScheduled() const = 0;
+  virtual Timestamp getNextTimeWindowStartTimestamp() const = 0;
+  virtual bool isMeasurementRelevant(const Sensor & s, Timestamp t) const = 0;
+
   virtual double secsSinceStart(Timestamp timestamp) const = 0;
   virtual std::string secsSinceStart(const Interval & interval) const = 0;