Switched to Eigen::Matrix/Vector for orientation.