Merge remote-tracking branch 'origin/develop' into fix/expression_leak
commit
1e2c29fac8
|
@ -28,6 +28,7 @@
|
|||
#include <cstdint>
|
||||
|
||||
#include <exception>
|
||||
#include <string>
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
#include <tbb/scalable_allocator.h>
|
||||
|
|
|
@ -67,8 +67,7 @@ class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
|
|||
boost::optional<Matrix&> H2 = boost::none) const override {
|
||||
const Point3 dir = Tb - Ta;
|
||||
Matrix33 H_predicted_dir;
|
||||
const Point3 predicted =
|
||||
dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr);
|
||||
const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
|
||||
if (H1) *H1 = -H_predicted_dir;
|
||||
if (H2) *H2 = H_predicted_dir;
|
||||
return predicted - measured_w_aZb_;
|
||||
|
|
Loading…
Reference in New Issue