diff --git a/gtsam.h b/gtsam.h index b1766e6af..75310e512 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2149,6 +2149,10 @@ virtual class DroneDynamicsVelXYFactor : gtsam::NoiseModelFactor { DroneDynamicsVelXYFactor(size_t key1, size_t key2, size_t key3, Vector motors, Vector acc, const gtsam::noiseModel::Base* noiseModel); }; +#include +virtual class CageFactor : gtsam::NoiseModelFactor { + CageFactor(size_t key1, const gtsam::Pose3& pose, double cageBoundary, const gtsam::noiseModel::Base* noiseModel); +}; #include template @@ -2461,22 +2465,22 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - static void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + static void predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - static gtsam::imuBias::ConstantBias PredictImuBias(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, + static gtsam::imuBias::ConstantBias predictImuBias(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - static gtsam::LieVector PredictVelocity(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, + static gtsam::LieVector predictVelocity(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - static gtsam::Pose3 PredictPose(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, + static gtsam::Pose3 predictPose(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); diff --git a/gtsam/slam/CageFactor.h b/gtsam/slam/CageFactor.h new file mode 100644 index 000000000..54a77b541 --- /dev/null +++ b/gtsam/slam/CageFactor.h @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file CageFactor.h + * @author Krunal Chande + * @date November 10, 2014 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Factor to constrain position based on size of the accessible area + */ + +class CageFactor: public NoiseModelFactor1 { +private: + Pose3 pose_; + double cageBoundary_; + typedef CageFactor This; + typedef NoiseModelFactor1 Base; + +public: + CageFactor() {} /* Default Constructor*/ + CageFactor(Key poseKey, const Pose3& pose, double cageBoundary, const SharedNoiseModel& model): + Base(model, poseKey), pose_(pose), cageBoundary_(cageBoundary){} + virtual ~CageFactor(){} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast(gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x) - z */ + Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { + + double distance = pose.translation().dist(Point3(0,0,0)); + if(distance > cageBoundary_){ + double distance = pose.range(Point3(0,0,0), H); + return (gtsam::Vector(1) << distance - cageBoundary_); + } else { + if(H) *H = gtsam::zeros(1, Pose3::Dim()); + return (gtsam::Vector(1) << 0.0); + } +// Point3 p2; +// double x = pose.x(), y = pose.y(), z = pose.z(); +// if (x < 0) x = -x; +// if (y < 0) y = -y; +// if (z < 0) z = -z; +// double errorX = 100/(x-cageBoundary_), errorY = 100/(y-cageBoundary_), errorZ = 100/(z-cageBoundary_); +// if (H) *H = pose.translation().distance(p2, H); +// return (Vector(3)< (&expected); + return e != NULL + && Base::equals(*e, tol) + ; + } + + /** print contents */ + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "Cage Factor, Cage Boundary = " << cageBoundary_ << " Pose: " << pose_ << std::endl; + Base::print("", keyFormatter); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(cageBoundary_); + ar & BOOST_SERIALIZATION_NVP(pose_); + } + +}; // end CageFactor +} // end namespace + + diff --git a/gtsam/slam/tests/testCageFactor.cpp b/gtsam/slam/tests/testCageFactor.cpp new file mode 100644 index 000000000..3379aa701 --- /dev/null +++ b/gtsam/slam/tests/testCageFactor.cpp @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCageFactor.cpp + * @brief Unit tests CageFactor class + * @author Krunal Chande + */ + + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model +static SharedNoiseModel model(noiseModel::Unit::Create(6)); + +LieVector factorError(const Pose3& pose, const CageFactor& factor) { + return factor.evaluateError(pose); +} + + +/* ************************************************************************* */ +TEST(CageFactor, Inside) { + Key poseKey(1); + Pose3 pose(Rot3::ypr(0,0,0),Point3(0,0,0)); + double cageBoundary = 10; // in m + CageFactor factor(poseKey, pose, cageBoundary, model); + + // Set the linearization point + Pose3 poseLin; + Matrix H; + Vector actualError(factor.evaluateError(poseLin, H)); + Vector expectedError = zero(1); + CHECK(assert_equal(expectedError, actualError, 1e-9)); + + // use numerical derivatives to calculate the jacobians + Matrix HExpected; + HExpected = numericalDerivative11(boost::bind(&factorError, _1, factor), pose); + CHECK(assert_equal(HExpected, H, 1e-9)); +} + +/* ************************************************************************* */ +TEST(CageFactor, Outside) { + Key poseKey(1); + Point3 translation = Point3(15,0,0); + Pose3 pose(Rot3::ypr(0,0,0),translation); + double cageBoundary = 10; // in m + CageFactor factor(poseKey, pose, cageBoundary, model); + + // Set the linearization point + Pose3 poseLin; + Matrix H; + Vector actualError(factor.evaluateError(pose, H)); + Vector expectedError(Vector(1)<<5); + CHECK(assert_equal(expectedError, actualError, 1e-9)); + + // use numerical derivatives to calculate the jacobians + Matrix HExpected; + HExpected = numericalDerivative11(boost::bind(&factorError, _1, factor), pose); + CHECK(assert_equal(HExpected, H, 1e-9)); +} +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */