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

Recover the convenient evaluateError() method

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

15
gtsam/sam/BearingFactor.h
View File

@ -60,6 +60,21 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
std::cout << s << "BearingFactor" << std::endl;
Base::print(s, kf);
}
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 = unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
private:
friend class boost::serialization::access;

14
gtsam/sam/BearingRangeFactor.h
View File

@ -66,6 +66,20 @@ class BearingRangeFactor
Expression<A2>(keys[1]));
}
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 = unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
/// print
void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const override {

14
gtsam/sam/RangeFactor.h
View File

@ -58,6 +58,20 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
Expression<A2> a2_(keys[1]);
return Expression<T>(Range<A1, A2>(), a1_, 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 = unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
/// print
void print(const std::string& s = "",

18
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -47,29 +47,25 @@ double measurement(10.0);
/* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point,
const RangeFactor2D& factor) {
const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorError3D(const Pose3& pose, const Point3& point,
const RangeFactor3D& factor) {
const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
const RangeFactorWithTransform2D& factor) {
const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
const RangeFactorWithTransform3D& factor) {
const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
@ -262,10 +258,8 @@ TEST( RangeFactor, Jacobian2D ) {
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
std::vector<Matrix> actualHs(2);
factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
const Matrix& H1Actual = actualHs.at(0);
const Matrix& H2Actual = actualHs.at(1);
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;