What is the mathematics behind the turtlebot calibration?
Because of problems with the calibration, I try to understand what is happening in the turtlebot_calibration node, specifically the scan_to_angle.py, method scan_cb(). This is a piece of code that attempts to calculate a single angle from a scan. The code is:
for r in msg.ranges:
if angle > self.min_angle and angle < self.max_angle and r < msg.range_max:
x = sin(angle) * r
y = cos(angle) * r
sum_x += x
sum_y += y
sum_xx += x*x
sum_xy += x*y
num += 1
angle += d_angle
if num > 0:
angle=atan2((-sum_x*sum_y+num*sum_xy)/(num*sum_xx-sum_x*sum_x), 1)
res = ScanAngle()
res.header = msg.header
res.scan_angle = angle
self.pub.publish(res)
else:
rospy.logerr("Please point me at a wall.")
Can someone tell me more about the mathematics behind this?
You may also notice that the algorithm uses the atan2() in a unintended way, causing a divide by zero in case of all x = 0 (unlikely) or num = 1.