Simplified the code using Eigen vector

Merged branch MoveToEigen