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

Merge pull request #955 from borglab/feature/improvedPoseToPointFactor 

Templated PoseToPointFactor
release/4.3a0
Frank Dellaert 2021-12-08 09:49:40 -05:00 committed by GitHub
parent 16dc333072 748b647a79
commit c2a9fc04b5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 178 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -1,11 +1,14 @@
/**
* @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 model relative position measurements
* from a (2D or 3D) pose to a landmark
* @author David Wisth
* @author Luca Carlone
**/
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -17,12 +20,13 @@ namespace gtsam {
* A class for a measurement between a pose and a point.
* @addtogroup SLAM
*/
class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
template<typename POSE = Pose3, typename POINT = Point3>
class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
private:
typedef PoseToPointFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base;
typedef NoiseModelFactor2<POSE, POINT> Base;
Point3 measured_; /** the point measurement in local coordinates */
POINT measured_; /** the point measurement in local coordinates */
public:
// shorthand for a smart pointer to a factor
@ -32,7 +36,7 @@ class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
PoseToPointFactor() {}
/** Constructor */
PoseToPointFactor(Key key1, Key key2, const Point3& measured,
PoseToPointFactor(Key key1, Key key2, const POINT& measured,
const SharedNoiseModel& model)
: Base(model, key1, key2), measured_(measured) {}
@ -54,26 +58,26 @@ class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
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);
traits<POINT>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors
* @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
* @brief Error = w_T_b.inverse()*w_P - measured_
* @param w_T_b The pose of the body in world coordinates
* @param w_P The estimated point location in world coordinates
*
* Note: measured_ and the error are in local coordiantes.
*/
Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
Vector evaluateError(const POSE& w_T_b, const POINT& w_P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return wTwi.transformTo(wPwp, H1, H2) - measured_;
return w_T_b.transformTo(w_P, H1, H2) - measured_;
}
/** return the measured */
const Point3& measured() const { return measured_; }
const POINT& measured() const { return measured_; }
private:
/** Serialization function */

161
gtsam_unstable/slam/tests/testPoseToPointFactor.cpp Normal file
View File

@ -0,0 +1,161 @@
/**
* @file testPoseToPointFactor.cpp
* @brief
* @author David Wisth
* @author Luca Carlone
* @date June 20, 2020
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/slam/PoseToPointFactor.h>
using namespace gtsam;
using namespace gtsam::noiseModel;
/* ************************************************************************* */
// Verify zero error when there is no noise
TEST(PoseToPointFactor, errorNoiseless_2D) {
Pose2 pose = Pose2::identity();
Point2 point(1.0, 2.0);
Point2 noise(0.0, 0.0);
Point2 measured = point + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
Isotropic::Sigma(2, 0.05));
Vector expectedError = Vector2(0.0, 0.0);
Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/* ************************************************************************* */
// Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise_2D) {
Pose2 pose = Pose2::identity();
Point2 point(1.0, 2.0);
Point2 noise(-1.0, 0.5);
Point2 measured = point + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
Isotropic::Sigma(2, 0.05));
Vector expectedError = -noise;
Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/* ************************************************************************* */
// Check Jacobians are correct
TEST(PoseToPointFactor, jacobian_2D) {
// Measurement
gtsam::Point2 l_meas(1, 2);
// Linearisation point
gtsam::Point2 p_t(-5, 12);
gtsam::Rot2 p_R(1.5 * M_PI);
Pose2 p(p_R, p_t);
gtsam::Point2 l(3, 0);
// Factor
Key pose_key(1);
Key point_key(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives
auto f = std::bind(&PoseToPointFactor<Pose2,Point2>::evaluateError, factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none);
Matrix numerical_H1 = numericalDerivative21<Vector, Pose2, Point2>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose2, Point2>(f, p, l);
// Use the factor to calculate the derivative
Matrix actual_H1;
Matrix actual_H2;
factor.evaluateError(p, l, actual_H1, actual_H2);
// Verify we get the expected error
EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
}
/* ************************************************************************* */
// Verify zero error when there is no noise
TEST(PoseToPointFactor, errorNoiseless_3D) {
Pose3 pose = Pose3::identity();
Point3 point(1.0, 2.0, 3.0);
Point3 noise(0.0, 0.0, 0.0);
Point3 measured = point + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor<Pose3,Point3> 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));
}
/* ************************************************************************* */
// Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise_3D) {
Pose3 pose = Pose3::identity();
Point3 point(1.0, 2.0, 3.0);
Point3 noise(-1.0, 0.5, 0.3);
Point3 measured = point + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor<Pose3,Point3> 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));
}
/* ************************************************************************* */
// Check Jacobians are correct
TEST(PoseToPointFactor, jacobian_3D) {
// Measurement
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);
Pose3 p(p_R, p_t);
gtsam::Point3 l = gtsam::Point3(3, 0, 5);
// Factor
Key pose_key(1);
Key point_key(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives
auto f = std::bind(&PoseToPointFactor<Pose3,Point3>::evaluateError, factor,
std::placeholders::_1, std::placeholders::_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);
// Use the factor to calculate the derivative
Matrix actual_H1;
Matrix actual_H2;
factor.evaluateError(p, l, actual_H1, actual_H2);
// Verify we get the expected error
EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -1,86 +0,0 @@
/**
* @file testPoseToPointFactor.cpp
* @brief
* @author David Wisth
* @date June 20, 2020
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/slam/PoseToPointFactor.h>
using namespace gtsam;
using namespace gtsam::noiseModel;
/// Verify zero error when there is no noise
TEST(PoseToPointFactor, errorNoiseless) {
Pose3 pose = Pose3::identity();
Point3 point(1.0, 2.0, 3.0);
Point3 noise(0.0, 0.0, 0.0);
Point3 measured = t + noise;
Key pose_key(1);
Key point_key(2);
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));
}
/// Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise) {
Pose3 pose = Pose3::identity();
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));
Vector expectedError = noise;
Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/// Check Jacobians are correct
TEST(PoseToPointFactor, jacobian) {
// Measurement
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);
Pose3 p(p_R, p_t);
gtsam::Point3 l = gtsam::Point3(3, 0, 5);
// Factor
Key pose_key(1);
Key point_key(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives
auto f = std::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);
// Use the factor to calculate the derivative
Matrix actual_H1;
Matrix actual_H2;
factor.evaluateError(p, l, actual_H1, actual_H2);
// Verify we get the expected error
EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8));
EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */