use rviz_helpers color
finish some utils function
estimate pose based on simple pnp