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

wrong drone's dynamics model for estimation used in the first icra submission

release/4.3a0
krunalchande 2014-10-02 15:02:16 -04:00
parent 53ac63d2f8
commit cb016fe405
4 changed files with 448 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
gtsam/slam/DroneDynamicsFactor.h Normal file
View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/*
* DroneDynamicsFactor.h
*
* Created on: Oct 1, 2014
* Author: krunal
*/
#pragma once
#include <boost/lexical_cast.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Binary factor for a range measurement
* @addtogroup SLAM
*/
class DroneDynamicsFactor: public NoiseModelFactor2<Pose3, LieVector> {
private:
LieVector measured_; /** body velocity measured from raw acc and motor inputs*/
typedef DroneDynamicsFactor This;
typedef NoiseModelFactor2<Pose3, LieVector> Base;
public:
DroneDynamicsFactor() {} /* Default constructor */
DroneDynamicsFactor(Key poseKey, Key velKey, const LieVector& measured,
const SharedNoiseModel& model) :
Base(model, poseKey, velKey), measured_(measured) {
}
virtual ~DroneDynamicsFactor() {}
/// @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, const LieVector& vel,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
// error = v - wRb*measured
Rot3 wRb = pose.rotation();
Vector3 error;
if (H1 || H2) {
*H2 = eye(3);
*H1 = zeros(3,6);
Matrix H1Rot;
error = wRb.unrotate(Point3(vel.vector()), H1Rot, H2).vector() - measured_.vector();
(*H1).block(0,0,3,3) = H1Rot;
}
else {
error = wRb.unrotate(Point3(vel.vector())).vector() - measured_.vector();
}
return error;
}
/** return the measured */
LieVector measured() const {
return measured_;
}
/** 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 << "DroneDynamicsFactor, measured = " << measured_.vector().transpose() << 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("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // DroneDynamicsFactor
} // namespace gtsam

124
gtsam/slam/DroneDynamicsVelXYFactor.h Normal file
View File

@ -0,0 +1,124 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/*
* DroneDynamicsVelXYFactor.h
*
* Created on: Oct 1, 2014
* Author: krunal
*/
#pragma once
#include <boost/lexical_cast.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Binary factor for a range measurement
* @addtogroup SLAM
*/
class DroneDynamicsVelXYFactor: public NoiseModelFactor3<Pose3, LieVector, LieVector> {
private:
Vector motors_; /** motor inputs */
Vector acc_; /** raw acc */
Matrix M_;
typedef DroneDynamicsVelXYFactor This;
typedef NoiseModelFactor3<Pose3, LieVector, LieVector> Base;
public:
DroneDynamicsVelXYFactor() {} /* Default constructor */
DroneDynamicsVelXYFactor(Key poseKey, Key velKey, Key cKey, const Vector& motors, const Vector& acc,
const SharedNoiseModel& model) :
Base(model, poseKey, velKey, cKey), motors_(motors), acc_(acc), M_(computeM(motors, acc)) {
}
virtual ~DroneDynamicsVelXYFactor() {}
/// @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))); }
// M = [sum(sqrt(m))ax 1 0 0; 0 0 sum(sqrt(m))ay 1; 0 0 0 0]
Matrix computeM(const Vector& motors, const Vector& acc) const {
Matrix M = zeros(3,4);
double sqrtSumMotors = sqrt(motors(0)) + sqrt(motors(1)) + sqrt(motors(2)) + sqrt(motors(3));
M(0,0) = sqrtSumMotors*acc(0); M(0, 1) = 1.0;
M(1,2) = 1.0; M(1, 3) = sqrtSumMotors*acc(1);
return M;
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const LieVector& vel, const LieVector& c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
// error = R'*v - M*c, where
Rot3 wRb = pose.rotation();
Vector error;
if (H1 || H2 || H3) {
*H1 = zeros(3, 6);
*H2 = eye(3);
Matrix H1Rot;
error = wRb.unrotate(Point3(vel.vector()), H1Rot, H2).vector() - M_*c.vector();
(*H1).block(0,0,3,3) = H1Rot;
*H3 = -M_;
}
else {
error = wRb.unrotate(Point3(vel.vector())).vector() - M_*c.vector();
}
return error;
}
/** 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 << "DroneDynamicsVelXYFactor, motors = " << motors_.transpose() << " acc: " << acc_.transpose() << 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("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(motors_);
ar & BOOST_SERIALIZATION_NVP(acc_);
}
}; // DroneDynamicsVelXYFactor
} // namespace gtsam

102
gtsam/slam/tests/testDroneDynamicsFactor.cpp Normal file
View File

@ -0,0 +1,102 @@
/* ----------------------------------------------------------------------------
* 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 testRangeFactor.cpp
* @brief Unit tests for DroneDynamicsFactor Class
* @author Stephen Williams
* @date Oct 2012
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/DroneDynamicsFactor.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 for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(3));
/* ************************************************************************* */
LieVector factorError(const Pose3& pose, const LieVector& vel, const DroneDynamicsFactor& factor) {
return factor.evaluateError(pose, vel);
}
/* ************************************************************************* */
TEST( DroneDynamicsFactor, Error) {
// Create a factor
Key poseKey(1);
Key velKey(2);
LieVector measurement((Vector(3)<<10.0, 1.5, 0.0));
DroneDynamicsFactor factor(poseKey, velKey, measurement, model);
// Set the linearization point
Pose3 pose(Rot3::ypr(1.0, 2.0, 0.57), Point3());
LieVector vel((Vector(3) <<
-2.913425624770731,
-2.200086236883632,
-9.429823523226959));
// Use the factor to calculate the error
Matrix H1, H2;
Vector actualError(factor.evaluateError(pose, vel, H1, H2));
Vector expectedError = zero(3);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError, _1, vel, factor), pose);
H2Expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&factorError, pose, _1, factor), vel);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1, 1e-9));
CHECK(assert_equal(H2Expected, H2, 1e-9));
}
/* *************************************************************************
TEST( DroneDynamicsFactor, Jacobian2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose2 pose(1.0, 2.0, 0.57);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* *************************************************************************
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

108
gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp Normal file
View File

@ -0,0 +1,108 @@
/* ----------------------------------------------------------------------------
* 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 testRangeFactor.cpp
* @brief Unit tests for DroneDynamicsVelXYFactor Class
* @author Stephen Williams
* @date Oct 2012
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/DroneDynamicsVelXYFactor.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 for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(3));
/* ************************************************************************* */
LieVector factorError(const Pose3& pose, const LieVector& vel, const LieVector& coeffs, const DroneDynamicsVelXYFactor& factor) {
return factor.evaluateError(pose, vel, coeffs);
}
/* ************************************************************************* */
TEST( DroneDynamicsVelXYFactor, Error) {
// Create a factor
Key poseKey(1);
Key velKey(2);
Key coeffsKey(3);
Vector motors = (Vector(4) << 179, 180, 167, 168)/256.0;
Vector3 acc = (Vector(3) << 2., 1., 3.);
DroneDynamicsVelXYFactor factor(poseKey, velKey, coeffsKey, motors, acc, model);
// Set the linearization point
Pose3 pose(Rot3::ypr(1.0, 2.0, 0.57), Point3());
LieVector vel((Vector(3) <<
-2.913425624770731,
-2.200086236883632,
-9.429823523226959));
LieVector coeffs((Vector(4) << -9.3, 2.7, -6.5, 1.2));
// Use the factor to calculate the error
Matrix H1, H2, H3;
Vector actualError(factor.evaluateError(pose, vel, coeffs, H1, H2, H3));
Vector expectedError = zero(3);
// Verify we get the expected error
// CHECK(assert_equal(expectedError, actualError, 1e-9));
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError, _1, vel, coeffs, factor), pose);
H2Expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&factorError, pose, _1, coeffs, factor), vel);
H3Expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&factorError, pose, vel, _1, factor), coeffs);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1, 1e-9));
CHECK(assert_equal(H2Expected, H2, 1e-9));
CHECK(assert_equal(H3Expected, H3, 1e-9));
}
/* *************************************************************************
TEST( DroneDynamicsVelXYFactor, Jacobian2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose2 pose(1.0, 2.0, 0.57);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* *************************************************************************
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */