diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 99d717681..b2d9c2d11 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -168,6 +169,13 @@ public: */ Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {} + /** Print */ + void print(const std::string& str = "") const { + std::cout << str; + permutation_.print(" permutation: "); + container_.print(" container: "); + } + /** Access the container through the permutation */ value_type& operator[](size_t index) { return container_[permutation_[index]]; }