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

fix evaluateError() methods

release/4.3a0
Jose Luis Blanco Claraco 2020-07-28 21:37:02 +02:00
parent 0ee5fc58f1
commit dcd9415ddf
No known key found for this signature in database
GPG Key ID: D443304FBD70A641
1 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/sam/RangeFactor.h
View File

@ -65,7 +65,7 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
{ {
std::vector<Matrix> Hs(2); std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys(); const auto &keys = Factor::keys();
const Vector error = unwhitenedError( const Vector error = Base::unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs); Hs);
if (H1) *H1 = Hs[0]; if (H1) *H1 = Hs[0];
@ -136,6 +136,20 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_); return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
} }
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = Base::unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
/** print contents */ /** print contents */
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {