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

81
gtsam_unstable/slam/PoseToPointFactor.h
View File

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

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

@ -5,9 +5,9 @@
* @date June 20, 2020
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/slam/PoseToPointFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using namespace gtsam::noiseModel;
@ -21,25 +21,27 @@ TEST(PoseToPointFactor, errorNoiseless) {
Key pose_key(1);
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 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
TEST(PoseToPointFactor, errorNoise) {
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 measured = t + noise;
Key pose_key(1);
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 actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError,actualError, 1E-5));
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/// Check Jacobians are correct
@ -48,8 +50,8 @@ TEST(PoseToPointFactor, jacobian) {
gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
// Linearisation point
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::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);
Pose3 p(p_R, p_t);
gtsam::Point3 l = gtsam::Point3(3, 0, 5);
@ -61,8 +63,8 @@ TEST(PoseToPointFactor, jacobian) {
PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives
boost::function<Vector(const Pose3&, const Point3&)> f = boost::bind(
&LandmarkFactor::evaluateError, factor, _1, _2, boost::none, boost::none);
auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
boost::none, boost::none);
Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);