Calm before the storm
Changed containers from std::vector to Eigen vectors