Found possible bug when normalizing quaternion

[voyage19941119]

Thanks for this amazing work leading the trend for monocular SLAM!

I found two possible bugs in the part that normalizing the quaternion after EKF. Please check and correct me if I was wrong.

• the Jacobian for dq_norm_by_dq. Specifically, in the function dqi_by_dqi() and dqi_by_dqj() in motion_model.cpp, the "qq" should all be "sqrt(qq)", according to my derivation

• the quaternion is not successfully normalized in function func_xvnorm_and_dxvnorm_by_dxv in motion_model.cpp. After this function , the quaternion in state vector is still not an unit quaternion. In function "func_xvnorm_and_dxvnorm_by_dxv",
  Tempqb should be divided by its norm before passing its value to "xvnorm"

xvnormRES_(3) = Tempqb.w();
xvnormRES_(4) = Tempqb.x();
xvnormRES_(5) = Tempqb.y();
xvnormRES_(6) = Tempqb.z();

When I run the Scenlib2, the quaternion norm is increasing by the time, this value is close to 1 during a short time, so the performance is not infected significantly. But when running in a long period, this could be a serious problem.