pythonを用いた角度計測

前提・実現したいこと

角度計測を実現したいです。
現在pythonとインテル社のrealsenseを用いて角度計測(ヨー方向、ピッチ方向)を行っています。しかしヨー方向の場合はrealsenseを壁と垂直に置いた場合は右方向しか測定できず、ピッチ方向の場合は上方向しか測定できません。(左方向と下方向はデータが0になります)

発生している問題・エラーメッセージ

カメラから見て左方向と下方向のみ角度が計測できません

エラーメッセージ

RuntimeWarning: overflow encountered in ushort_scalars
runtimewarning: invalid value encountered in double_scalars
のエラーが出ます

該当のソースコード

python
1ソースコード

試したこと

M1_pc_z = dframe[100,320]
M1_pc_x = ((320 - intr.ppx) * (dframe[100,320] / 1000) / intr.fx) *1000 #mm
M1_pc_y = ((100 - intr.ppy) * (dframe[100,320] /1000) / intr.fy) *1000 #mm

M2_pc_z = dframe[360,120]
M2_pc_x = ((120 - intr.ppx) * (dframe[360,120] / 1000) / intr.fx) *1000  #mm
M2_pc_y = ((360 - intr.ppy) * (dframe[360,120] /1000) / intr.fy) *1000 #mm

M3_pc_z = dframe[360,520]
M3_pc_x = ((520 - intr.ppx) * (dframe[360,520] / 1000) / intr.fx) *1000  #mm
M3_pc_y = ((360 - intr.ppy) * (dframe[360,520] /1000) / intr.fy) *1000 #mm

Mc_pc_z = dframe[int(intr.ppy),int(intr.ppx)]
Mc_pc_x = ((intr.ppx - intr.ppx) * (dframe[int(intr.ppy),int(intr.ppx)] / 1000) / intr.fx) *1000  #mm
Mc_pc_y = ((intr.ppy - intr.ppy) * (dframe[int(intr.ppy),int(intr.ppx)] /1000) / intr.fy) *1000 #mm




cv2.circle(cframe, (int(intr.ppx), int(intr.ppy)), 5, (0, 0, 255), -1) # red

pitch
ap = abs((M2_pc_y) - (M1_pc_y))
bp = abs((M1_pc_z) - (M2_pc_z))

cp = math.atan(ap/bp)



yaw
ay= abs((M3_pc_z) - (M2_pc_z))
by = abs((M3_pc_x) - (M2_pc_x))
cy = math.atan(ay/by)

abs関数を使って絶対値が取られるためエラーが出ているのかもしれません。
ご協力をお願いします。