Fixed boxPlus operator
Fixed rotation matrix diff finiteDifference
last fix