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

Merge pull request #956 from borglab/fix/compiler-warnings-posetopointfactor

release/4.3a0
Varun Agrawal 2021-12-09 09:37:40 -05:00 committed by GitHub
parent b47f46a6f5 45d5328457
commit 3615c4b5ac
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -45,8 +45,8 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n" << keyFormatter(this->key2()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl; << " measured: " << measured_.transpose() << std::endl;
@ -54,8 +54,8 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const { double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) && return e != nullptr && Base::equals(*e, tol) &&
traits<POINT>::Equals(this->measured_, e->measured_, tol); traits<POINT>::Equals(this->measured_, e->measured_, tol);
@ -70,9 +70,10 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
* *
* Note: measured_ and the error are in local coordiantes. * Note: measured_ and the error are in local coordiantes.
*/ */
Vector evaluateError(const POSE& w_T_b, const POINT& w_P, Vector evaluateError(
boost::optional<Matrix&> H1 = boost::none, const POSE& w_T_b, const POINT& w_P,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
return w_T_b.transformTo(w_P, H1, H2) - measured_; return w_T_b.transformTo(w_P, H1, H2) - measured_;
} }