A real-time system for three dimensional simultaneous localization and mapping (SLAM) in mobile robots based on visual data of camera using incremental smoothing and mapping algorithm, the state-of-the-art solution of SLAM problem - iSAM2, is developed and implemented in this thesis. Building 3D maps of environments is an important and essential task for mobile robots in navigation and manipulation. An autonomous mobile robot in unknown environment is exposed to considerable uncertainties and faced three main problems; localization, mapping and motion planning, which in many cases should be solved simultaneously. SLAM involves finding a proper description of the environment as a map and exploring the robot position on it. SLAM problems is in fact categorized as a probabilistic robotic problems which deal with uncertainties in robot perception and manipulation and solves the robot localization and mapping. The key idea in these set of problems is to represent uncertainties using probability theory. There has been different solutions to SLAM problem which can be classified in three main methods: the Kalman filter and its family, the particle filters and the graphical SLAM solution. One of the newest graphical based solutions is iSAM which is based on fast incremental matrix factorization. Updating a QR factorization of the sparse smoothing information matrix, iSAM provides an exact solution. In this method only the matrix entries that actually affects the computation are considered. This is efficient for trajectories with many loops. In order to build a 3D map of an environment, despite the iSAM algorithm, 3D data of the environment is needed. The RGB-D camera is used to capture RGB images along with per-pixel depth information. From the concepts in computer vision, by extracting features from consecutive frames and their matching, the movement of the features is extracted. Using the computer vision concepts and consequently integrating the RGB-D data and the iSAM method, the 3D map of environment is built. In autonomous navigation tasks, a robot can plan collision-free paths only for those areas that have been covered by sensor measurements and detected. However, the unmapped areas have to be avoided and should be included in the map somehow. Furthermore, the knowledge about unmapped areas is essential during exploration. As maps are created autonomously, the robot proceed the measurements in the previously unmapped areas. To represent the map in 3D, a volumetric representation of space named octomap is used. This mapping approach is based on octrees and uses probabilistic occupancy estimation which not only represents occupied space, but also free and unknown areas. Through this representation the map can be updated efficiently and the memory equipment can be kept at a minimum. The developed system is implemented on Experia, a mobile robot platform in Advanced Robotics and Mechatronics Laboratory (ARMLab) of the mechanical department of the Isfahan University of Technology, by using Microsoft Kinect Xbox 360 as the RGB-D camera. Moreover, by the fusion of the Kinect and IMU data, the orientation of the robot in roll and pitch angels are enhanced. Through this fusion, the robot localize itself better and consequently, the output 3D map of the system is improved. Keywords: Autonomous Mobile Robot, Simultaneous Localization and Mapping, Least Square Problem, Incremental Smoothing and Mapping, iSAM2 Algorithm, Octomap, 3D Mapping