Normalize normal vector before including in pose matrix

[marip8]

reach/src/plugin_utils.cpp

Line 158 in d89ddaf

p.matrix().col(2).head<3>() = norm;