Add Problem::transformAll()

include/core/problem/problem.h

@@ -364,6 +364,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         // All branches -------------------------------------------
         // perturb states
         void perturb(double amplitude = 0.01);
+        void transformAll(const VectorComposite& _transformation);
 
         // Covariances
         void clearCovariance();

src/problem/problem.cpp

@@ -1255,4 +1255,24 @@ void Problem::perturb(double amplitude)
         L->perturb(amplitude);
 }
 
+void Problem::transformAll(const VectorComposite& _transformation)
+{
+    // TODO: uncomment all calls to transform() below:
+    // Sensors
+    for (auto S : getHardware()->getSensorList())
+        S->transform(_transformation);
+
+    // Frames and Captures
+    for (auto F : getTrajectory()->getFrameMap())
+    {
+        F.second->transform(_transformation);
+        for (auto& C : F.second->getCaptureList())
+            C->transform(_transformation);
+    }
+
+    // Landmarks
+    for (auto L : getMap()->getLandmarkList())
+        L->transform(_transformation);
+}
+
 } // namespace wolf