Pose estimation and 3D reconstruction of environment are essential technics in robotics and computer vision. In this paper we present a method for camera tracking and 3D reconstruction of static environments, using a ToF sensor which provides less reliable depth information. Based on a primary camera pose, we eliminate outlier in distance measurements. Subsequently, we estimate camera pose again using only inlier data. A voxel grid map is updated by integrating depth measurement with a truncated signed distance function. It is represented as 3D environment reconstruction. Our method is an attractive extending of the pose estimation in outdoor environment. In outdoor environment, 3D range cameras cannot measure the distance or they provide inaccurate distance measurement. The experiments were carried out both in indoor and outdoor and we analyze the results of the proposed methods which use a ToF camera in comparison with a previous approach.