i m new to c++ and ros and want to build my own mapping algorithm using laser scan and scan matching

i have recently started working on ros and just built one code that subscribes laserscan msgs and convert them to x y and z coordinate. Now i want to print the pointcloud in rviz and use it in scan matching using icp and then building the map of the environment .