fix a bug that ex cannot be locked when setting estimate_extrinsic to 0

IOdissey · Oct 9, 2019 · 9ec9283 · 9ec9283
1 parent 6766e1c
commit 9ec9283
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/vins_estimator/src/estimator/estimator.cpp b/vins_estimator/src/estimator/estimator.cpp
@@ -869,7 +869,7 @@ void Estimator::double2vector()
             ric[i] = Quaterniond(para_Ex_Pose[i][6],
                                  para_Ex_Pose[i][3],
                                  para_Ex_Pose[i][4],
-                                 para_Ex_Pose[i][5]).toRotationMatrix();
+                                 para_Ex_Pose[i][5]).normalized().toRotationMatrix();
         }
     }