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

Added and tested Cage Factor. Added Matlab Wrapper

release/4.3a0
krunalchande 2014-11-15 19:08:44 -05:00
parent 3ba997014d
commit d49396c1d2
3 changed files with 184 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam.h
View File

@ -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 <gtsam/slam/CageFactor.h>
virtual class CageFactor : gtsam::NoiseModelFactor {
CageFactor(size_t key1, const gtsam::Pose3& pose, double cageBoundary, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/BearingFactor.h>
template<POSE, POINT, ROTATION>
@ -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);

98
gtsam/slam/CageFactor.h Normal file
View File

@ -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 <boost/lexical_cast.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Factor to constrain position based on size of the accessible area
*/
class CageFactor: public NoiseModelFactor1<Pose3> {
private:
Pose3 pose_;
double cageBoundary_;
typedef CageFactor This;
typedef NoiseModelFactor1<Pose3> 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>(gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** h(x) - z */
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> 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)<<errorX, errorY, errorZ);
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(cageBoundary_);
ar & BOOST_SERIALIZATION_NVP(pose_);
}
}; // end CageFactor
} // end namespace

78
gtsam/slam/tests/testCageFactor.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/slam/CageFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>
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<Pose3>(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<Pose3>(boost::bind(&factorError, _1, factor), pose);
CHECK(assert_equal(HExpected, H, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */