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

Made Velocity a Vector3

release/4.3a0
dellaert 2015-07-17 17:36:29 -07:00
parent d5bf2493fe
commit 233cabb3fb
7 changed files with 29 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -38,13 +38,14 @@ Vector PoseRTV::vector() const {
Vector rtv(9); Vector rtv(9);
rtv.head(3) = rotation().xyz(); rtv.head(3) = rotation().xyz();
rtv.segment(3,3) = translation().vector(); rtv.segment(3,3) = translation().vector();
rtv.tail(3) = velocity().vector(); rtv.tail(3) = velocity();
return rtv; return rtv;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool PoseRTV::equals(const PoseRTV& other, double tol) const { bool PoseRTV::equals(const PoseRTV& other, double tol) const {
return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol); return pose().equals(other.pose(), tol)
&& equal_with_abs_tol(velocity(), other.velocity(), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -52,7 +53,7 @@ void PoseRTV::print(const string& s) const {
cout << s << ":" << endl; cout << s << ":" << endl;
gtsam::print((Vector)R().xyz(), " R:rpy"); gtsam::print((Vector)R().xyz(), " R:rpy");
t().print(" T"); t().print(" T");
velocity().print(" V"); gtsam::print((Vector)velocity(), " V");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -137,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
Vector6 imu; Vector6 imu;
// acceleration // acceleration
Vector3 accel = (v2-v1).vector() / dt; Vector3 accel = (v2-v1) / dt;
imu.head<3>() = r2.transpose() * (accel - kGravity); imu.head<3>() = r2.transpose() * (accel - kGravity);
// rotation rates // rotation rates
@ -160,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const { Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
// predict point for constraint // predict point for constraint
// NOTE: uses simple Euler approach for prediction // NOTE: uses simple Euler approach for prediction
Point3 pred_t2 = t() + v2 * dt; Point3 pred_t2 = t().retract(v2 * dt);
return pred_t2; return pred_t2;
} }
@ -187,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
// Note that we rotate the velocity // Note that we rotate the velocity
Matrix3 D_newvel_R, D_newvel_v; Matrix3 D_newvel_R, D_newvel_v;
Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v); Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector();
if (Dglobal) { if (Dglobal) {
Dglobal->setZero(); Dglobal->setZero();

4
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -13,7 +13,7 @@
namespace gtsam { namespace gtsam {
/// Syntactic sugar to clarify components /// Syntactic sugar to clarify components
typedef Point3 Velocity3; typedef Vector3 Velocity3;
/** /**
* Robot state for use with IMU measurements * Robot state for use with IMU measurements
@ -66,7 +66,7 @@ public:
// and avoidance of Point3 // and avoidance of Point3
Vector vector() const; Vector vector() const;
Vector translationVec() const { return pose().translation().vector(); } Vector translationVec() const { return pose().translation().vector(); }
Vector velocityVec() const { return velocity().vector(); } const Velocity3& velocityVec() const { return velocity(); }
// testable // testable
bool equals(const PoseRTV& other, double tol=1e-6) const; bool equals(const PoseRTV& other, double tol=1e-6) const;

11
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -106,12 +106,13 @@ private:
static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
double dt, const dynamics::IntegrationMode& mode) { double dt, const dynamics::IntegrationMode& mode) {
const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); const Velocity3& v1 = x1.v(), v2 = x2.v();
Velocity3 hx; const Point3& p1 = x1.t(), p2 = x2.t();
Point3 hx;
switch(mode) { switch(mode) {
case dynamics::TRAPEZOIDAL: hx = p1 + (v1 + v2) * dt *0.5; break; case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break;
case dynamics::EULER_START: hx = p1 + v1 * dt; break; case dynamics::EULER_START: hx = p1.retract(v1 * dt); break;
case dynamics::EULER_END : hx = p1 + v2 * dt; break; case dynamics::EULER_END : hx = p1.retract(v2 * dt); break;
default: assert(false); break; default: assert(false); break;
} }
return (p2 - hx).vector(); return (p2 - hx).vector();

12
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create a simple chain of poses to generate IMU measurements // create a simple chain of poses to generate IMU measurements
const double dt = 1.0; const double dt = 1.0;
PoseRTV pose1, PoseRTV pose1,
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Point3(2.0, 2.0, 0.0)), pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Point3(0.0, 0.0, 0.0)), pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Point3(2.0, 2.0, 0.0)); pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
// create measurements // create measurements
SharedDiagonal model = noiseModel::Unit::Create(6); SharedDiagonal model = noiseModel::Unit::Create(6);
@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create a simple chain of poses to generate IMU measurements // create a simple chain of poses to generate IMU measurements
const double dt = 1.0; const double dt = 1.0;
PoseRTV pose1, PoseRTV pose1,
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)); pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
// create measurements // create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);

4
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -198,7 +198,7 @@ TEST( testPoseRTV, transformed_from_1 ) {
Matrix actDTrans, actDGlobal; Matrix actDTrans, actDGlobal;
PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
EXPECT(assert_equal(expected, actual, tol)); EXPECT(assert_equal(expected, actual, tol));
Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
@ -217,7 +217,7 @@ TEST( testPoseRTV, transformed_from_2 ) {
Matrix actDTrans, actDGlobal; Matrix actDTrans, actDGlobal;
PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
EXPECT(assert_equal(expected, actual, tol)); EXPECT(assert_equal(expected, actual, tol));
Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails

4
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
const double dt = 1.0; const double dt = 1.0;
PoseRTV origin, PoseRTV origin,
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)), pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
pose1a(Point3(0.5, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)),
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)); pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testVelocityConstraint, trapezoidal ) { TEST( testVelocityConstraint, trapezoidal ) {

8
gtsam_unstable/gtsam_unstable.h
View File

@ -53,9 +53,9 @@ class Dummy {
class PoseRTV { class PoseRTV {
PoseRTV(); PoseRTV();
PoseRTV(Vector rtv); PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel);
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel); PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel);
PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel); PoseRTV(const gtsam::Pose3& pose, const Vector& vel);
PoseRTV(const gtsam::Pose3& pose); PoseRTV(const gtsam::Pose3& pose);
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
@ -66,7 +66,7 @@ class PoseRTV {
// access // access
gtsam::Point3 translation() const; gtsam::Point3 translation() const;
gtsam::Rot3 rotation() const; gtsam::Rot3 rotation() const;
gtsam::Point3 velocity() const; Vector velocity() const;
gtsam::Pose3 pose() const; gtsam::Pose3 pose() const;
// Vector interfaces // Vector interfaces