The railway industry is constantly growing to meet the demand of society for stable, accessible and sustainable transportation. With this growth, the need for the railway to be reliable increases, requiring frequent surveying and maintenance. Fugro's RILA (Rail Infrastructure aLignment Acquisition) mobile mapping system contributes by making the surveying more accessible to the relevant railway network stakeholders. However, the system has its limitations in environments where the GNSS (Global Navigation Satelite System) signal is occluded, such as tunnels and underground stations. The geo-data collected by RILA in those areas is poorly georeferenced due to the poorly tracked trajectory of the system, which introduces spatial data misalignment up to a meter or more. The current methods to fix data misalignment rely on manual data corrections, which is not cost-effective, or on automatic solutions, which have limited applicability. Thus, the aim of this research is to develop an improved trajectory optimization method, thereby ensuring accurate geo-referencing and alignment of the survey data. This thesis proposes a newly developed methodology to achieve this aim: features are extracted from point cloud surveys, matched and utilized by g2o optimizer and GNSS processing software to optimize the trajectory. The development is described and results are evaluated on two different scales - locally, within a point cloud tile and globally, within a sequence of tiles. It is done by using Glasgow's underground railway network as a test case. Results from the implementation demonstrate significant improvements in trajectory accuracy - a misalignment of point cloud data was reduced from a 1.5 m to a cm level within an optimization time frame that took approximately 10 hours. This improvement in accuracy was present under different complex environments using both the local and global versions of the algorithm. However, the area near the railway tunnel entrance saw a limited benefit from the implementation of the proposed algorithm. In conclusion, the developed trajectory optimization algorithm optimizes the trajectory and improves the alignment of the survey data. Moreover, the method outperforms the currently employed solutions by being automatic and applicable in different environments. However, further research is required to optimize the algorithm itself (accuracy and computationally speed of the algorithm) and to more accurately define its limitations in terms of the surveyed environments.