Combining two laser scans into one coordinate system

For my current Boeing project I have to use two laser scanners to get 360 degree fov. I need to extract walls from the room and localize based on the walls. I decided to combine the laser scans from both lasers into one coordiante system and then extract walls. I wrote few matlab scripts to combine the scans and extract lines etc. After being satisfied with the results I moved to implement the same in C++ specifically in ROS. When I reimplemented I got very weird results when I combined both laser scans into one coordinate system.

After debugging for almost a day and taking help from a friend I found the BUG! I wish it was a programming bug, sadly it was a mistake in my understanding. I tried to put as much as possible in the image below. Once we knew the problem the formulas were elementary math but realizing that we needed to do this took time. I partially blame my matlab implementation which was also wrong but the error was never noticeable in the plots! At the end of the day though, things are looking great and I am glad one bridge is crossed :-)

After implementing it, the program started crashing arbitrarily giving segmentation fault! And that lead us to another "double precision" issue!! The calculation involves taking inverse sin by using asin. asin gives a HUGE number for any input greater than 1. In case of asin(a/b) for a=b due to double precision problems the function returned a crappy number. So, I had to make a minor modification so that a is always less than b :-) A productive day involving lot of debugging and learning!