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

Add more documentation and clang-format

release/4.3a0
David 2020-06-20 09:45:24 +10:00
parent c422815b94
commit 0ee4e3b77e
2 changed files with 50 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -1,15 +1,15 @@
/** /**
* @file PoseToPointFactor.hpp * @file PoseToPointFactor.hpp
* @brief This factor can be used to track a 3D landmark over time by providing * @brief This factor can be used to track a 3D landmark over time by
* local measurements of its location. *providing local measurements of its location.
* @author David Wisth * @author David Wisth
**/ **/
#pragma once #pragma once
#include <ostream>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <ostream>
namespace gtsam { namespace gtsam {
@ -17,17 +17,14 @@ namespace gtsam {
* A class for a measurement between a pose and a point. * A class for a measurement between a pose and a point.
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class PoseToPointFactor: public NoiseModelFactor2<Pose3, Point3> { class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
private:
private:
typedef PoseToPointFactor This; typedef PoseToPointFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base; typedef NoiseModelFactor2<Pose3, Point3> Base;
Point3 measured_; /** the point measurement */ Point3 measured_; /** the point measurement in local coordinates */
public:
public:
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<PoseToPointFactor> shared_ptr; typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
@ -36,60 +33,58 @@ public:
/** Constructor */ /** Constructor */
PoseToPointFactor(Key key1, Key key2, const Point3& measured, PoseToPointFactor(Key key1, Key key2, const Point3& measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model)
Base(model, key1, key2), measured_(measured) : Base(model, key1, key2), measured_(measured) {}
{
}
virtual ~PoseToPointFactor() {} virtual ~PoseToPointFactor() {}
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
std::cout << s << "PoseToPointFactor(" DefaultKeyFormatter) const {
<< 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;
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected,
const This *e = dynamic_cast<const This*> (&expected); double tol = 1e-9) const {
return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol); const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(this->measured_, e->measured_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors /** vector of errors
* Error = pose_est.inverse()*point_est - measured_ * @brief Error = wTwi.inverse()*wPwp - measured_
* (The error is in the measurement coordinate system) * @param wTwi The pose of the sensor in world coordinates
* @param wPwp The estimated point location in world coordinates
*
* Note: measured_ and the error are in local coordiantes.
*/ */
Vector evaluateError(const Pose3& W_T_WI, Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
const Point3& W_r_WC,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const boost::optional<Matrix&> H2 = boost::none) const {
{ return wTwi.transformTo(wPwp, H1, H2) - measured_;
return W_T_WI.transformTo(W_r_WC, H1, H2) - measured_;
} }
/** return the measured */ /** return the measured */
const Point3& measured() const { const Point3& measured() const { return measured_; }
return measured_;
}
private:
private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar& BOOST_SERIALIZATION_NVP(measured_);
} }
}; // \class PoseToPointFactor }; // \class PoseToPointFactor
} /// namespace gtsam } // namespace gtsam

20
gtsam_unstable/slam/tests/testPoseToPointFactor.h
View File

@ -5,9 +5,9 @@
* @date June 20, 2020 * @date June 20, 2020
*/ */
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/slam/PoseToPointFactor.h> #include <gtsam_unstable/slam/PoseToPointFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel; using namespace gtsam::noiseModel;
@ -21,25 +21,27 @@ TEST(PoseToPointFactor, errorNoiseless) {
Key pose_key(1); Key pose_key(1);
Key point_key(2); Key point_key(2);
PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); PoseToPointFactor factor(pose_key, point_key, measured,
Isotropic::Sigma(3, 0.05));
Vector expectedError = Vector3(0.0, 0.0, 0.0); Vector expectedError = Vector3(0.0, 0.0, 0.0);
Vector actualError = factor.evaluateError(pose, point); Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError,actualError, 1E-5)); EXPECT(assert_equal(expectedError, actualError, 1E-5));
} }
/// Verify expected error in test scenario /// Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise) { TEST(PoseToPointFactor, errorNoise) {
Pose3 pose = Pose3::identity(); Pose3 pose = Pose3::identity();
Point3 point(1.0 , 2.0, 3.0); Point3 point(1.0, 2.0, 3.0);
Point3 noise(-1.0, 0.5, 0.3); Point3 noise(-1.0, 0.5, 0.3);
Point3 measured = t + noise; Point3 measured = t + noise;
Key pose_key(1); Key pose_key(1);
Key point_key(2); Key point_key(2);
PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); PoseToPointFactor factor(pose_key, point_key, measured,
Isotropic::Sigma(3, 0.05));
Vector expectedError = noise; Vector expectedError = noise;
Vector actualError = factor.evaluateError(pose, point); Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError,actualError, 1E-5)); EXPECT(assert_equal(expectedError, actualError, 1E-5));
} }
/// Check Jacobians are correct /// Check Jacobians are correct
@ -49,7 +51,7 @@ TEST(PoseToPointFactor, jacobian) {
// Linearisation point // Linearisation point
gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5*M_PI, -0.3*M_PI, 0.4*M_PI); gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
Pose3 p(p_R, p_t); Pose3 p(p_R, p_t);
gtsam::Point3 l = gtsam::Point3(3, 0, 5); gtsam::Point3 l = gtsam::Point3(3, 0, 5);
@ -61,8 +63,8 @@ TEST(PoseToPointFactor, jacobian) {
PoseToPointFactor factor(pose_key, point_key, l_meas, noise); PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives // Calculate numerical derivatives
boost::function<Vector(const Pose3&, const Point3&)> f = boost::bind( auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
&LandmarkFactor::evaluateError, factor, _1, _2, boost::none, boost::none); boost::none, boost::none);
Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l); Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l); Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);