Cleaned up part of codes, added additional tbb::parallel_for loops, converted 2d vectors into Mat