start of Eigen::Matrix based smoothing version