12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix TranslationFactor with Vector3 as Point3

release/4.3a0
Fan Jiang 2020-07-04 20:31:04 -04:00
parent dde41ebf05
commit 258d05c9ef
1 changed files with 1 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/sfm/TranslationFactor.h
View File

@ -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_;