wrong drone's dynamics model for estimation used in the first icra submission
krunalchande
parent
53ac63d2f8
commit
cb016fe405
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue